﻿#include "CPlateB.h"
#include "CHVisionAdvX.h"



CPlateB::CPlateB()
{
}


CPlateB::~CPlateB()
{
}




s_Rtnf CPlateB::OnDetect(s_Image3dS Image3d, s_DefectPlateBPara spara1, s_DefectPlateBRtsPara& spara2)
{

	HTuple time_start = 0.0;
	HTuple time_end = 0.0;
	CountSeconds(&time_start);

	s_Rtnf sError;
	sError.iCode = -1;
	sError.strInfo = "";


	//复位结果数据
	spara2.Reset();



	// Local iconic variables 

	HObject ho_Region, ho_PointCloud;
	HObject ho_Normals, ho_X, ho_Y, ho_Gray, ho_Color;
	HObject ho_Z, ho_NX, ho_NY, ho_NZ, ho_MultiChannelImage, ho_NzRender;
	HObject ho_Region1, ho_Region2, ho_RegionNZ;
	HObject ho_Region3, ho_Region4, ho_RegionNXY;
	HObject ho_RegionRegionNXYZ, ho_RegionIntersection;
	HObject ho_ImageReduced, ho_Xs, ho_Ys, ho_Zs;
	HObject ho_Domain, ho_RegionFillUp, ho_RegionClosing;
	HObject ho_RegionOpening, ho_RegionErosion, ho_RegionTb, ho_RegionQg;
	HObject ho_X1, ho_Y1, ho_Z1, ho_ROI;
	HObject ho_ImageSurface, ho_ImageSub, ho_ConnectedRegions;
	HObject ho_SelectedRegions, ho_RectangleQg, ho_RectangleL;
	HObject ho_RectangleR, ho_RectangleU, ho_RectangleD;
	HObject ho_RegionDifference, ho_RectangleQgL;
	HObject ho_RectangleQgR, ho_RectangleQgU, ho_RectangleQgD;
	HObject ho_RectangleLAdv, ho_RectangleRAdv, ho_RectangleUAdv;
	HObject ho_RectangleDAdv, ho_RectangleCAdv, ho_RectanglePartList;
	HObject ho_RectangleQgSort, ho_SortedRegions;
	HObject ho_ImageSubZ1, ho_X1PZeroC, ho_Y1PZeroC;
	HObject ho_Z1PZeroC, ho_RegionRoiCBulge, ho_ImageSubC;
	HObject ho_Z1PZero, ho_X1PZero, ho_Y1PZero;
	HObject ho_RegionRoiCBulgeUnion, ho_RectangleQgEx;
	HObject ho_RectangleLAdvRemoveQg, ho_X1PZeroL;
	HObject ho_Y1PZeroL, ho_Z1PZeroL, ho_Z1PZeroLRemoveQg;
	HObject ho_RegionQgExL, ho_RegionRoiLBulgeRemoveQg;
	HObject ho_ImageSubLRemoveQg, ho_RegionUnion;
	HObject ho_RegionRoiLBulgeQg, ho_ImageSubLQg;
	HObject ho_RectangleRAdvRemoveQg, ho_X1PZeroR;
	HObject ho_Y1PZeroR, ho_Z1PZeroR, ho_Z1PZeroRRemoveQg;
	HObject ho_RegionQgExR, ho_RegionRoiRBulgeRemoveQg;
	HObject ho_ImageSubRRemoveQg, ho_RegionRoiRBulgeQg;
	HObject ho_ImageSubRQg, ho_RectangleUAdvRemoveQg;
	HObject ho_X1PZeroU, ho_Y1PZeroU, ho_Z1PZeroU;
	HObject ho_Z1PZeroURemoveQg, ho_RegionQgExU, ho_RegionRoiUBulgeRemoveQg;
	HObject ho_ImageSubURemoveQg, ho_RegionRoiUBulgeQg;
	HObject ho_ImageSubUQg, ho_RectangleDAdvRemoveQg;
	HObject ho_X1PZeroD, ho_Y1PZeroD, ho_Z1PZeroD;
	HObject ho_Z1PZeroDRemoveQg, ho_RegionQgExD, ho_RegionRoiDBulgeRemoveQg;
	HObject ho_ImageSubDRemoveQg, ho_RegionRoiDBulgeQg;
	HObject ho_ImageSubDQg, ho_RegionRoiLBulge, ho_RegionRoiRBulge;
	HObject ho_RegionRoiUBulge, ho_RegionRoiDBulge, ho_RegionBulgeAll, ho_RegionBulgeAllUnion;
	HObject ho_RegionPartLz, ho_RectPartLz, ho_RectPartLzAdv;
	HObject hImageRed, hImageGreen, hImageBlue;


	HObject ho_ImageZ1ZeroReal, ho_ZZeroRealC, ho_ZZeroRealL, ho_ZZeroRealR, ho_ZZeroRealU, ho_ZZeroRealD;
	HObject ho_ZZeroRealLQg, ho_ZZeroRealRQg, ho_ZZeroRealUQg, ho_ZZeroRealDQg;
	HObject ho_RegionMoved;

	HObject ho_ROI_0, ho_ROI_1, ho_ROI_2, ho_ROI_3;
	HObject ho_ROI_4, ho_ROI_5, ho_ROI_6, ho_ROI_7;
	HObject ho_ROI_8, ho_ROI_9, ho_ROI_10, ho_ROI_11;
	HObject ho_ROI_12, ho_ROI_13, ho_ROI_14, ho_ROI_15;
	HObject ho_RegionQgIn;


	HObject ho_RegionQgReal, ho_ObjectSelected1, ho_ObjectSelected2, ho_EmptyRegion;
	HObject ho_XZero, ho_YZero, ho_ZZero;

	HObject ho_ImageScaled, ho_ImageScaleMax, ho_ImageClear, ho_ImageR, ho_ImageG, ho_ImageB, ho_Multichannel;
	HObject ho_Z1PZeroRender, ho_Z1ZeroLzRender;

	// Local control variables 

	HTuple hv_Rows, hv_Columns;
	HTuple hv_Max, hv_ObjectModel3DConnected;
	HTuple hv_ObjectModel3DSelected, hv_UnionObjectModel3D;
	HTuple hv_Om3DScene, hv_fThNZAbsVal;
	HTuple hv_fThNXAbsVal, hv_fThNYAbsVal;
	HTuple hv_fThGrayVal, hv_ObjectModel3D1;
	HTuple hv_GenParamValue, hv_ObjectModel3DAdv;
	HTuple hv_Om3DNoLz, hv_Pose, hv_Om3DSceneZero;

	HTuple hv_Alpha, hv_Beta;
	HTuple hv_Gamma, hv_Area;
	HTuple hv_Row, hv_Column;
	HTuple hv_Width, hv_Height;
	HTuple hv_Row1, hv_Column1;
	HTuple hv_Row2, hv_Column2;
	HTuple hv_Diameter, hv_Rectangularity;
	HTuple hv_fQgOffsetL, hv_fQgOffsetR;
	HTuple hv_fQgOffsetU, hv_fQgOffsetD;
	HTuple hv_ColumnL, hv_ColumnR;
	HTuple hv_RowU, hv_RowD;
	HTuple hv_Om3DC, hv_SampledObjectModel3D;
	HTuple hv_OM3DPlane, hv_ParFitting;
	HTuple hv_ValFitting, hv_ObjectModel3DOutID;
	HTuple hv_GenParamPosePlane, hv_PlaneDatum;
	HTuple hv_PoseTbC, hv_PoseInvert;
	HTuple hv_HomMat3D, hv_OM3dPZeroC;
	HTuple hv_fThBulgeLowerC, hv_fThBulgeHigerC;
	HTuple hv_fThBulgeLowerS, hv_fThBulgeHigerS;
	HTuple hv_Grayval, hv_fQgHOrg;
	HTuple hv_fQgLHOrg, hv_fQgRHOrg, hv_fQgUHOrg, hv_fQgDHOrg;

	HTuple hv_fQgLExCol, hv_fQgLExRow;
	HTuple hv_fQgRExCol, hv_fQgRExRow;
	HTuple hv_fQgUExCol, hv_fQgUExRow;
	HTuple hv_fQgDExCol, hv_fQgDExRow;



	HTuple hv_fQgThOffset, hv_Om3DL;
	HTuple hv_Om3DLRemoveQg;
	HTuple hv_OM3dPZeroL, hv_Om3DR;
	HTuple hv_Om3DRRemoveQg;
	HTuple hv_OM3dPZeroR, hv_Om3DU;
	HTuple hv_Om3DURemoveQg;
	HTuple hv_OM3dPZeroU, hv_Om3DD;
	HTuple hv_Om3DDRemoveQg;
	HTuple hv_OM3dPZeroD, hv_fConnectionDis;

	HTuple hv_OM3dPZero;


	HTuple hChannels;

	HTuple hv_bFind;
	HTuple hv_Number1;
	HTuple hv_Number2;
	HTuple hv_i;
	HTuple hv_j;

	HTuple hv_ColumnL0, hv_ColumnR0, hv_RowU0, hv_RowD0;
	HTuple hv_Min,  hv_Range;
	HTuple hv_fRenderRangeScale, hv_fDisMaxAbs;

	while (true)
	{
		try
		{

			ho_X = Image3d.X.Clone();
			ho_Y = Image3d.Y.Clone();
			ho_Z = Image3d.Z.Clone();
			ho_Gray = Image3d.Gray.Clone();
			ho_Color = Image3d.Color.Clone();

			ho_NX = Image3d.NX.Clone();
			ho_NY = Image3d.NY.Clone();
			ho_NZ = Image3d.NZ.Clone();
			

			//new HVisionAdv().WriteImage3DX(Image3d,@"D:\\lins","1");

			//判断输入
			if (!ho_Z.IsInitialized() || ho_Z.CountObj() == 0 || !ho_X.IsInitialized() || ho_X.CountObj() == 0 || !ho_Y.IsInitialized() || ho_Y.CountObj() == 0)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "图像为空!";
				break;
			}


			if (!ho_NZ.IsInitialized() || ho_NZ.CountObj() == 0 || !ho_NX.IsInitialized() || ho_NX.CountObj() == 0 || !ho_NY.IsInitialized() || ho_NY.CountObj() == 0)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "图像为空!";
				break;
			}


			if (!ho_Gray.IsInitialized() || ho_Gray.CountObj() == 0)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "图像为空!";
				break;
			}


			//3通道图像转成单通道
			CountChannels(ho_Gray, &hChannels);
			if (hChannels[0].I() == 3)
			{
				Decompose3(ho_Gray, &hImageRed, &hImageGreen, &hImageBlue);
				ho_Gray = hImageRed.Clone();
			}

			//算法处理

			CHVisionAdvX lCHVisionAdvXObj;

			//step0 手动输入气缸的大致区域



			GenRectangle1(&ho_ROI_0, 118.05, 668.665, 295.147, 743.868);
			GenRectangle1(&ho_ROI_1, 127.743, 920.781, 304.84, 995.984);
			GenRectangle1(&ho_ROI_2, 131.48, 1172.87, 308.577, 1248.07);
			GenRectangle1(&ho_ROI_3, 128.989, 1427.44, 290, 1490);


			GenRectangle1(&ho_ROI_4, 302.28, 1456.72, 353.347, 1614.43);
			GenRectangle1(&ho_ROI_5, 542.669, 1462.3, 593.737, 1615.67);
			GenRectangle1(&ho_ROI_6, 799.251, 1463.55, 850.319, 1616.91);
			GenRectangle1(&ho_ROI_7, 1049.61, 1480, 1100.67, 1613.19);



			GenRectangle1(&ho_ROI_8, 1115.85, 1436.16, 1270.5, 1487.99);
			GenRectangle1(&ho_ROI_9, 1114.88, 1186.12, 1269.53, 1237.94);
			GenRectangle1(&ho_ROI_10, 1120.49, 926.944, 1275.14, 978.773);
			GenRectangle1(&ho_ROI_11, 1119.61, 680.977, 1274.26, 732.806);



			GenRectangle1(&ho_ROI_12, 1059.94, 527.714, 1102.24, 683.2);
			GenRectangle1(&ho_ROI_13, 805.849, 527.714, 848.146, 683.2);
			GenRectangle1(&ho_ROI_14, 549.417, 526.835, 591.715, 682.322);
			GenRectangle1(&ho_ROI_15, 312.764, 523.11, 355.062, 678.596);


			GenEmptyObj(&ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_0, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_1, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_2, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_3, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_4, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_5, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_6, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_7, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_8, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_9, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_10, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_11, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_12, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_13, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_14, &ho_RegionQgIn);
			ConcatObj(ho_RegionQgIn, ho_ROI_15, &ho_RegionQgIn);


			//step1 各种ROI区域/点云

			//铜板整体区域
			XyzToObjectModel3d(ho_X, ho_Y, ho_Z, &hv_Om3DScene);

			//提取铜板整体参数
			hv_fThNZAbsVal = /*0.8*/spara1.fThNZAbsVal;

			Compose3(ho_NX, ho_NY, ho_NZ, &ho_NzRender);
			ScaleImageMax(ho_NzRender, &ho_NzRender);

			Threshold(ho_NZ, &ho_Region1, -1, -hv_fThNZAbsVal);
			Threshold(ho_NZ, &ho_Region2, hv_fThNZAbsVal, 1);
			Union2(ho_Region1, ho_Region2, &ho_RegionNZ);
			ReduceDomain(ho_Z, ho_RegionNZ, &ho_ImageReduced);

			XyzToObjectModel3d(ho_X, ho_Y, ho_ImageReduced, &hv_ObjectModel3D1);
		    ConnectionObjectModel3d(hv_ObjectModel3D1, "distance_3d", 5, &hv_ObjectModel3DConnected);
			GetObjectModel3dParams(hv_ObjectModel3DConnected, "num_points", &hv_GenParamValue);
			TupleMax(hv_GenParamValue, &hv_Max);
			SelectObjectModel3d(hv_ObjectModel3DConnected, "num_points", "and", hv_Max, hv_Max,&hv_ObjectModel3DSelected);
	
			ObjectModel3dToXyz(&ho_Xs, &ho_Ys, &ho_Zs, hv_ObjectModel3DSelected, "from_xyz_map",HTuple(), HTuple());
			GetDomain(ho_Xs, &ho_Domain);
			FillUp(ho_Domain, &ho_RegionFillUp);
			ClosingRectangle1(ho_RegionFillUp, &ho_RegionClosing, 101, 101);
			OpeningRectangle1(ho_RegionClosing, &ho_RegionOpening, 151, 151);

			ErosionRectangle1(ho_RegionOpening, &ho_RegionErosion, 7, 7);
			FillUp(ho_RegionErosion, &ho_RegionTb);

			ReduceObjectModel3dByView(ho_RegionTb, hv_Om3DScene, "xyz_mapping", HTuple(),&hv_ObjectModel3DSelected);
			CopyObjectModel3d(hv_ObjectModel3DSelected, "all", &hv_ObjectModel3DAdv);
			ObjectModel3dToXyz(&ho_X1, &ho_Y1, &ho_Z1, hv_ObjectModel3DAdv, "from_xyz_map",HTuple(), HTuple());


			//气缸区域

			Threshold(ho_NZ, &ho_Region1, -1, -0.85);
			Threshold(ho_NZ, &ho_Region2, 0.85, 1);
			Union2(ho_Region1, ho_Region2, &ho_RegionNZ);
			Intersection(ho_RegionNZ, ho_RegionTb, &ho_RegionIntersection);

			ReduceObjectModel3dByView(ho_RegionIntersection, hv_Om3DScene, "xyz_mapping",
				HTuple(), &hv_Om3DNoLz);
			SampleObjectModel3d(hv_Om3DNoLz, "xyz_mapping", 10, HTuple(), HTuple(), &hv_SampledObjectModel3D);

			CopyObjectModel3d(hv_SampledObjectModel3D, "all", &hv_OM3DPlane);
			hv_ParFitting.Clear();
			hv_ParFitting[0] = "primitive_type";
			hv_ParFitting[1] = "fitting_algorithm";
			hv_ValFitting.Clear();
			hv_ValFitting[0] = "plane";
			hv_ValFitting[1] = "least_squares_tukey";
			FitPrimitivesObjectModel3d(hv_OM3DPlane, hv_ParFitting, hv_ValFitting, &hv_ObjectModel3DOutID);
			GetObjectModel3dParams(hv_ObjectModel3DOutID, "primitive_pose", &hv_GenParamPosePlane);
			GenPlaneObjectModel3d(hv_GenParamPosePlane, (((HTuple(-1).Append(-1)).Append(1)).Append(1)) * 900,(((HTuple(-1).Append(1)).Append(1)).Append(-1)) * 900, &hv_PlaneDatum);
		
			trans_pose_face_x(hv_GenParamPosePlane, 0, &hv_Pose);

			PoseInvert(hv_Pose, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DScene, hv_HomMat3D, &hv_Om3DSceneZero);
			ObjectModel3dToXyz(&ho_XZero, &ho_YZero, &ho_ZZero, hv_Om3DSceneZero, "from_xyz_map",HTuple(), HTuple());

			Intersection(ho_RegionNZ, ho_ZZero, &ho_RegionIntersection);
			ReduceDomain(ho_ZZero, ho_RegionIntersection, &ho_ImageSub);

			Threshold(ho_ImageSub, &ho_Region, -100, -20);
			Intersection(ho_Region, ho_RegionQgIn, &ho_RegionIntersection);
			FillUp(ho_RegionIntersection, &ho_RegionFillUp);

			ClosingRectangle1(ho_RegionFillUp, &ho_RegionClosing, 11, 11);

			Connection(ho_RegionClosing, &ho_ConnectedRegions);
			DiameterRegion(ho_ConnectedRegions, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2,&hv_Diameter);
			Rectangularity(ho_ConnectedRegions, &hv_Rectangularity);

			SelectShape(ho_ConnectedRegions, &ho_SelectedRegions, (HTuple("max_diameter").Append("rectangularity")),
				"and", (HTuple(50).Append(0.6)), (HTuple(150).Append(1)));
			OpeningRectangle1(ho_SelectedRegions, &ho_RegionOpening, 5, 5);
			SmallestRectangle1(ho_RegionOpening, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleQg, hv_Row1, hv_Column1, hv_Row2, hv_Column2);

			ho_RegionQg = ho_RegionOpening;


			//子区域划分

			SmallestRectangle1(ho_RegionTb, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleL, hv_Row1 + 50, hv_Column1 - 100, hv_Row2 - 50, hv_Column1 + 100);
			GenRectangle1(&ho_RectangleR, hv_Row1 + 50, hv_Column2 - 95, hv_Row2 - 50, hv_Column2 + 100);
			GenRectangle1(&ho_RectangleU, hv_Row1 - 10, hv_Column1 - 100, hv_Row1 + 75, hv_Column2 + 100);
			GenRectangle1(&ho_RectangleD, hv_Row2 - 50, hv_Column1 - 100, hv_Row2 + 100, hv_Column2 + 100);
			Difference(ho_RectangleU, ho_RectangleL, &ho_RegionDifference);
			Difference(ho_RegionDifference, ho_RectangleR, &ho_RectangleU);
			Difference(ho_RectangleD, ho_RectangleL, &ho_RegionDifference);
			Difference(ho_RegionDifference, ho_RectangleR, &ho_RectangleD);

			Intersection(ho_RectangleL, ho_RectangleQg, &ho_RectangleQgL);
			Intersection(ho_RectangleR, ho_RectangleQg, &ho_RectangleQgR);
			Intersection(ho_RectangleU, ho_RectangleQg, &ho_RectangleQgU);
			Intersection(ho_RectangleD, ho_RectangleQg, &ho_RectangleQgD);


			hv_fQgOffsetL = /*25*/spara1.fQgOffsetL;
			hv_fQgOffsetR = /*25*/spara1.fQgOffsetR;
			hv_fQgOffsetU =/* 25*/spara1.fQgOffsetU;
			hv_fQgOffsetD = /*25*/spara1.fQgOffsetD;


			SmallestRectangle1(ho_RectangleQgL, &hv_Row1, &hv_Column1, &hv_Row2, &hv_ColumnL0);
			SmallestRectangle1(ho_RectangleQgR, &hv_Row1, &hv_ColumnR0, &hv_Row2, &hv_Column2);
			SmallestRectangle1(ho_RectangleQgU, &hv_Row1, &hv_Column1, &hv_RowU0, &hv_Column2);
			SmallestRectangle1(ho_RectangleQgD, &hv_RowD0, &hv_Column1, &hv_Row2, &hv_Column2);

			hv_ColumnL = hv_ColumnL0 + hv_fQgOffsetL;

			SmallestRectangle1(ho_RectangleL, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleLAdv, hv_RowU0 + hv_fQgOffsetU, hv_Column1, hv_RowD0 - hv_fQgOffsetD,hv_ColumnL);


			hv_ColumnR = hv_ColumnR0 - hv_fQgOffsetR;
			SmallestRectangle1(ho_RectangleR, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleRAdv, hv_RowU0 + hv_fQgOffsetU, hv_ColumnR, hv_RowD0 - hv_fQgOffsetD,hv_Column2);

			SmallestRectangle1(ho_RectangleU, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleUAdv, hv_Row1, hv_Column1, hv_RowU0 + hv_fQgOffsetU,hv_Column2);

			SmallestRectangle1(ho_RectangleD, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			GenRectangle1(&ho_RectangleDAdv, hv_RowD0 - hv_fQgOffsetD, hv_Column1, hv_Row2,hv_Column2);


			GenRectangle1(&ho_RectangleCAdv, hv_RowU0 + hv_fQgOffsetU, hv_ColumnL0 + hv_fQgOffsetL,hv_RowD0 - hv_fQgOffsetD, hv_ColumnR0 - hv_fQgOffsetR);

			GenEmptyObj(&ho_RectanglePartList);
			ConcatObj(ho_RectanglePartList, ho_RectangleCAdv, &ho_RectanglePartList);
			ConcatObj(ho_RectanglePartList, ho_RectangleLAdv, &ho_RectanglePartList);
			ConcatObj(ho_RectanglePartList, ho_RectangleRAdv, &ho_RectanglePartList);
			ConcatObj(ho_RectanglePartList, ho_RectangleUAdv, &ho_RectanglePartList);
			ConcatObj(ho_RectanglePartList, ho_RectangleDAdv, &ho_RectanglePartList);



			//气缸排序2
			GenEmptyObj(&ho_RectangleQgSort);
			GenEmptyObj(&ho_RegionQgReal);
			ConcatObj(ho_RegionQgReal, ho_RectangleQgU, &ho_RegionQgReal);
			ConcatObj(ho_RegionQgReal, ho_RectangleQgR, &ho_RegionQgReal);
			ConcatObj(ho_RegionQgReal, ho_RectangleQgD, &ho_RegionQgReal);
			ConcatObj(ho_RegionQgReal, ho_RectangleQgL, &ho_RegionQgReal);

			CountObj(ho_RegionQgIn, &hv_Number1);
			CountObj(ho_RegionQgReal, &hv_Number2);
			{
				HTuple end_val390 = hv_Number1;
				HTuple step_val390 = 1;
				for (hv_i = 1; hv_i.Continue(end_val390, step_val390); hv_i += step_val390)
				{
					SelectObj(ho_RegionQgIn, &ho_ObjectSelected1, hv_i);

					hv_bFind = 0;
					{
						HTuple end_val394 = hv_Number2;
						HTuple step_val394 = 1;
						for (hv_j = 1; hv_j.Continue(end_val394, step_val394); hv_j += step_val394)
						{
							SelectObj(ho_RegionQgReal, &ho_ObjectSelected2, hv_j);

							Intersection(ho_ObjectSelected2, ho_ObjectSelected1, &ho_RegionIntersection
							);
							AreaCenter(ho_RegionIntersection, &hv_Area, &hv_Row, &hv_Column);
							if (0 != (int(hv_Area > 100)))
							{
								hv_bFind = 1;
								ConcatObj(ho_RectangleQgSort, ho_ObjectSelected1, &ho_RectangleQgSort);
								break;
							}


						}
					}

					if (0 != (int(hv_bFind == 0)))
					{
						GenEmptyRegion(&ho_EmptyRegion);
						ConcatObj(ho_RectangleQgSort, ho_EmptyRegion, &ho_RectangleQgSort);
					}


				}
			}



			//***************************************************************************************************
			//step2  得到整版的粒子高度分布图
			//提取粒子高度区域 包含气缸 分区提取

			GetImageSize(ho_Z1, &hv_Width, &hv_Height);
			GenImageConst(&ho_ImageSubZ1, "real", hv_Width, hv_Height);
			GenImageConst(&ho_ImageZ1ZeroReal, "real", hv_Width, hv_Height);

			//中间区域粒子提取
			ReduceObjectModel3dByView(ho_RectangleCAdv, hv_ObjectModel3DAdv, "xyz_mapping", HTuple(), &hv_Om3DC);
			SampleObjectModel3d(hv_Om3DC, "fast", 5, HTuple(), HTuple(), &hv_SampledObjectModel3D);
			CopyObjectModel3d(hv_SampledObjectModel3D, "all", &hv_OM3DPlane);
			hv_ParFitting.Clear();
			hv_ParFitting[0] = "primitive_type";
			hv_ParFitting[1] = "fitting_algorithm";
			hv_ValFitting.Clear();
			hv_ValFitting[0] = "plane";
			hv_ValFitting[1] = "least_squares_tukey";

			FitPrimitivesObjectModel3d(hv_OM3DPlane, hv_ParFitting, hv_ValFitting, &hv_ObjectModel3DOutID);
			GetObjectModel3dParams(hv_ObjectModel3DOutID, "primitive_pose", &hv_GenParamPosePlane);
			GenPlaneObjectModel3d(hv_GenParamPosePlane, (((HTuple(-1).Append(-1)).Append(1)).Append(1)) * 900,
				(((HTuple(-1).Append(1)).Append(1)).Append(-1)) * 900, &hv_PlaneDatum);


			trans_pose_face_x(hv_GenParamPosePlane, 0, &hv_PoseTbC);//远离原点方向为正方向 

			PoseInvert(hv_PoseTbC, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DC, hv_HomMat3D, &hv_OM3dPZeroC);

			ObjectModel3dToXyz(&ho_X1PZeroC, &ho_Y1PZeroC, &ho_Z1PZeroC, hv_OM3dPZeroC, "from_xyz_map", HTuple(), HTuple());


			//用中间区域的调平矩阵调平整个面板
			AffineTransObjectModel3d(hv_ObjectModel3DAdv, hv_HomMat3D, &hv_OM3dPZero);
			ObjectModel3dToXyz(&ho_X1PZero, &ho_Y1PZero, &ho_Z1PZero, hv_OM3dPZero, "from_xyz_map", HTuple(), HTuple());

			hv_fThBulgeLowerC = /*1.5*/spara1.fThBulgeLowerC;
			hv_fThBulgeHigerC =/* 3*/spara1.fThBulgeHigerC;

			GenEmptyObj(&ho_RegionRoiCBulge);
			get_plane_ranger_z_out_region_x(ho_Z1PZeroC, &ho_RegionRoiCBulge, &ho_ImageSubC, &ho_ZZeroRealC, hv_fThBulgeLowerC, hv_fThBulgeHigerC, 50, 61, 61, 41);
			Union1(ho_RegionRoiCBulge, &ho_RegionRoiCBulgeUnion);
			GetRegionPoints(ho_RegionRoiCBulgeUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubC, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);
			GetGrayval(ho_ZZeroRealC, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);


			//气缸区域粒子提取

			hv_fQgThOffset = 4.5;


			//气缸区域粒子提取-L
			hv_fQgLHOrg = /*50.0*/spara1.fQgLHOrg;
			hv_fQgLExCol =/* 35*/spara1.fQgLExCol;
			hv_fQgLExRow = /*21*/spara1.fQgLExRow;


			ReduceObjectModel3dByView(ho_RectangleLAdv, hv_ObjectModel3DAdv, "xyz_mapping",HTuple(), &hv_Om3DL);

			DilationRectangle1(ho_RectangleQg, &ho_RectangleQgEx, hv_fQgLExCol, hv_fQgLExRow);

			Difference(ho_RectangleLAdv, ho_RectangleQgEx, &ho_RectangleLAdvRemoveQg);
			ReduceObjectModel3dByView(ho_RectangleLAdvRemoveQg, hv_ObjectModel3DAdv, "xyz_mapping",HTuple(), &hv_Om3DLRemoveQg);

			PoseInvert(hv_PoseTbC, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DL, hv_HomMat3D, &hv_OM3dPZeroL);
	
			ObjectModel3dToXyz(&ho_X1PZeroL, &ho_Y1PZeroL, &ho_Z1PZeroL, hv_OM3dPZeroL, "from_xyz_map",HTuple(), HTuple());

			Difference(ho_Z1PZeroL, ho_RectangleQgEx, &ho_RegionDifference);
			ReduceDomain(ho_Z1PZeroL, ho_RegionDifference, &ho_Z1PZeroLRemoveQg);
			Intersection(ho_Z1PZeroL, ho_RegionQg, &ho_RegionQgExL);


			//*需要单独开放提取参数
			hv_fThBulgeLowerS = /*3.0*/spara1.fThBulgeLowerS;
			hv_fThBulgeHigerS = /*4*/spara1.fThBulgeHigerS;

			GenEmptyObj(&ho_RegionRoiLBulgeRemoveQg);
			get_plane_ranger_z_out_region_x(ho_Z1PZeroLRemoveQg, &ho_RegionRoiLBulgeRemoveQg,
				&ho_ImageSubLRemoveQg, &ho_ZZeroRealL, hv_fThBulgeLowerS, hv_fThBulgeHigerS,
				50, 61, 61, 41);

			Union1(ho_RegionRoiLBulgeRemoveQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubLRemoveQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);
			GetGrayval(ho_ZZeroRealL, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);

			GenEmptyObj(&ho_RegionRoiLBulgeQg);
			get_qg_z_out_region_x(ho_Z1PZeroL, ho_RegionQgExL, &ho_ImageSubLQg, &ho_RegionRoiLBulgeQg,
				&ho_ZZeroRealLQg, hv_fQgLHOrg, hv_fQgThOffset, 100);




			Union1(ho_RegionRoiLBulgeQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubLQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);

			GetGrayval(ho_ZZeroRealLQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);


			//气缸区域粒子提取-R

			hv_fQgRHOrg = /*50.0*/spara1.fQgRHOrg;
			hv_fQgRExCol =/* 35*/spara1.fQgRExCol;
			hv_fQgRExRow = /*21*/spara1.fQgRExRow;


			ReduceObjectModel3dByView(ho_RectangleRAdv, hv_ObjectModel3DAdv, "xyz_mapping",HTuple(), &hv_Om3DR);
			DilationRectangle1(ho_RectangleQg, &ho_RectangleQgEx, hv_fQgRExCol, hv_fQgRExRow);

			Difference(ho_RectangleRAdv, ho_RectangleQgEx, &ho_RectangleRAdvRemoveQg);
			ReduceObjectModel3dByView(ho_RectangleRAdvRemoveQg, hv_ObjectModel3DAdv, "xyz_mapping",HTuple(), &hv_Om3DRRemoveQg);


			PoseInvert(hv_PoseTbC, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DR, hv_HomMat3D, &hv_OM3dPZeroR);
		
			ObjectModel3dToXyz(&ho_X1PZeroR, &ho_Y1PZeroR, &ho_Z1PZeroR, hv_OM3dPZeroR, "from_xyz_map",	HTuple(), HTuple());


			Difference(ho_Z1PZeroR, ho_RectangleQgEx, &ho_RegionDifference);
			ReduceDomain(ho_Z1PZeroR, ho_RegionDifference, &ho_Z1PZeroRRemoveQg);
			Intersection(ho_Z1PZeroR, ho_RegionQg, &ho_RegionQgExR);

			GenEmptyObj(&ho_RegionRoiRBulgeRemoveQg);
			get_plane_ranger_z_out_region_x(ho_Z1PZeroRRemoveQg, &ho_RegionRoiRBulgeRemoveQg,
				&ho_ImageSubRRemoveQg, &ho_ZZeroRealR, hv_fThBulgeLowerS, hv_fThBulgeHigerS,
				50, 61, 61, 41);

			Union1(ho_RegionRoiRBulgeRemoveQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubRRemoveQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);
			GetGrayval(ho_ZZeroRealR, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);

			GenEmptyObj(&ho_RegionRoiRBulgeQg);
			get_qg_z_out_region_x(ho_Z1PZeroR, ho_RegionQgExR, &ho_ImageSubRQg, &ho_RegionRoiRBulgeQg,
				&ho_ZZeroRealRQg, hv_fQgRHOrg, hv_fQgThOffset, 100);


			Union1(ho_RegionRoiRBulgeQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubRQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);

			GetGrayval(ho_ZZeroRealRQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);


			//气缸区域粒子提取-U

			hv_fQgUHOrg = /*50.0*/spara1.fQgUHOrg;
			hv_fQgUExCol = /*27*/spara1.fQgUExCol;
			hv_fQgUExRow =/* 35*/spara1.fQgUExRow;

			ReduceObjectModel3dByView(ho_RectangleUAdv, hv_ObjectModel3DAdv, "xyz_mapping",
				HTuple(), &hv_Om3DU);

			DilationRectangle1(ho_RectangleQg, &ho_RectangleQgEx, hv_fQgUExCol, hv_fQgUExRow);

			Difference(ho_RectangleUAdv, ho_RectangleQgEx, &ho_RectangleUAdvRemoveQg);
			ReduceObjectModel3dByView(ho_RectangleUAdvRemoveQg, hv_ObjectModel3DAdv, "xyz_mapping",
				HTuple(), &hv_Om3DURemoveQg);

			PoseInvert(hv_PoseTbC, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DU, hv_HomMat3D, &hv_OM3dPZeroU);

			ObjectModel3dToXyz(&ho_X1PZeroU, &ho_Y1PZeroU, &ho_Z1PZeroU, hv_OM3dPZeroU, "from_xyz_map",HTuple(), HTuple());

			Difference(ho_Z1PZeroU, ho_RectangleQgEx, &ho_RegionDifference);
			ReduceDomain(ho_Z1PZeroU, ho_RegionDifference, &ho_Z1PZeroURemoveQg);
			Intersection(ho_Z1PZeroU, ho_RegionQg, &ho_RegionQgExU);


			GenEmptyObj(&ho_RegionRoiUBulgeRemoveQg);
			get_plane_ranger_z_out_region_x(ho_Z1PZeroURemoveQg, &ho_RegionRoiUBulgeRemoveQg,
				&ho_ImageSubURemoveQg, &ho_ZZeroRealU, hv_fThBulgeLowerS, hv_fThBulgeHigerS,
				50, 61, 61, 41);

			Union1(ho_RegionRoiUBulgeRemoveQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubURemoveQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);
			GetGrayval(ho_ZZeroRealU, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);



			GenEmptyObj(&ho_RegionRoiUBulgeQg);
			get_qg_z_out_region_x(ho_Z1PZeroU, ho_RegionQgExU, &ho_ImageSubUQg, &ho_RegionRoiUBulgeQg,
				&ho_ZZeroRealUQg, hv_fQgUHOrg, hv_fQgThOffset, 100);


			Union1(ho_RegionRoiUBulgeQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubUQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);

			GetGrayval(ho_ZZeroRealUQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);

			//气缸区域粒子提取-D
			hv_fQgDHOrg = /*50.0*/spara1.fQgDHOrg;
			hv_fQgDExCol =/* 25*/spara1.fQgDExCol;
			hv_fQgDExRow = /*35*/spara1.fQgDExRow;
			ReduceObjectModel3dByView(ho_RectangleDAdv, hv_ObjectModel3DAdv, "xyz_mapping",HTuple(), &hv_Om3DD);

			DilationRectangle1(ho_RectangleQg, &ho_RectangleQgEx, hv_fQgDExCol, hv_fQgDExRow);

			Difference(ho_RectangleDAdv, ho_RectangleQgEx, &ho_RectangleDAdvRemoveQg);
			ReduceObjectModel3dByView(ho_RectangleDAdvRemoveQg, hv_ObjectModel3DAdv, "xyz_mapping",
				HTuple(), &hv_Om3DDRemoveQg);

			PoseInvert(hv_PoseTbC, &hv_PoseInvert);
			PoseToHomMat3d(hv_PoseInvert, &hv_HomMat3D);
			AffineTransObjectModel3d(hv_Om3DD, hv_HomMat3D, &hv_OM3dPZeroD);

			ObjectModel3dToXyz(&ho_X1PZeroD, &ho_Y1PZeroD, &ho_Z1PZeroD, hv_OM3dPZeroD, "from_xyz_map",HTuple(), HTuple());



			Difference(ho_Z1PZeroD, ho_RectangleQgEx, &ho_RegionDifference);
			ReduceDomain(ho_Z1PZeroD, ho_RegionDifference, &ho_Z1PZeroDRemoveQg);
			Intersection(ho_Z1PZeroD, ho_RegionQg, &ho_RegionQgExD);


			GenEmptyObj(&ho_RegionRoiDBulgeRemoveQg);
			get_plane_ranger_z_out_region_x(ho_Z1PZeroDRemoveQg, &ho_RegionRoiDBulgeRemoveQg,
				&ho_ImageSubDRemoveQg, &ho_ZZeroRealD, hv_fThBulgeLowerS, hv_fThBulgeHigerS,
				50, 61, 61, 41);

			Union1(ho_RegionRoiDBulgeRemoveQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubDRemoveQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);
			GetGrayval(ho_ZZeroRealD, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);


			GenEmptyObj(&ho_RegionRoiDBulgeQg);
			get_qg_z_out_region_x(ho_Z1PZeroD, ho_RegionQgExD, &ho_ImageSubDQg, &ho_RegionRoiDBulgeQg,
				&ho_ZZeroRealDQg, hv_fQgDHOrg, hv_fQgThOffset, 100);


			Union1(ho_RegionRoiDBulgeQg, &ho_RegionUnion);
			GetRegionPoints(ho_RegionUnion, &hv_Rows, &hv_Columns);
			GetGrayval(ho_ImageSubDQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageSubZ1, hv_Rows, hv_Columns, hv_Grayval);

			GetGrayval(ho_ZZeroRealDQg, hv_Rows, hv_Columns, &hv_Grayval);
			SetGrayval(ho_ImageZ1ZeroReal, hv_Rows, hv_Columns, hv_Grayval);



			//气缸区域合并一下
			GenEmptyObj(&ho_RegionRoiLBulge);
			ConcatObj(ho_RegionRoiLBulge, ho_RegionRoiLBulgeRemoveQg, &ho_RegionRoiLBulge
			);
			ConcatObj(ho_RegionRoiLBulge, ho_RegionRoiLBulgeQg, &ho_RegionRoiLBulge);

			GenEmptyObj(&ho_RegionRoiRBulge);
			ConcatObj(ho_RegionRoiRBulge, ho_RegionRoiRBulgeRemoveQg, &ho_RegionRoiRBulge
			);
			ConcatObj(ho_RegionRoiRBulge, ho_RegionRoiRBulgeQg, &ho_RegionRoiRBulge);


			GenEmptyObj(&ho_RegionRoiUBulge);
			ConcatObj(ho_RegionRoiUBulge, ho_RegionRoiUBulgeRemoveQg, &ho_RegionRoiUBulge
			);
			ConcatObj(ho_RegionRoiUBulge, ho_RegionRoiUBulgeQg, &ho_RegionRoiUBulge);

			GenEmptyObj(&ho_RegionRoiDBulge);
			ConcatObj(ho_RegionRoiDBulge, ho_RegionRoiDBulgeRemoveQg, &ho_RegionRoiDBulge
			);
			ConcatObj(ho_RegionRoiDBulge, ho_RegionRoiDBulgeQg, &ho_RegionRoiDBulge);

			GenEmptyObj(&ho_RegionBulgeAll);
			ConcatObj(ho_RegionBulgeAll, ho_RegionRoiCBulge, &ho_RegionBulgeAll);
			ConcatObj(ho_RegionBulgeAll, ho_RegionRoiLBulge, &ho_RegionBulgeAll);
			ConcatObj(ho_RegionBulgeAll, ho_RegionRoiRBulge, &ho_RegionBulgeAll);
			ConcatObj(ho_RegionBulgeAll, ho_RegionRoiUBulge, &ho_RegionBulgeAll);
			ConcatObj(ho_RegionBulgeAll, ho_RegionRoiDBulge, &ho_RegionBulgeAll);
			Union1(ho_RegionBulgeAll, &ho_RegionBulgeAllUnion);
			Connection(ho_RegionBulgeAllUnion, &ho_ConnectedRegions);
			SelectShape(ho_ConnectedRegions, &ho_RegionBulgeAll, "area", "and", 6, 99999999);


			//***************************************************************************************************
            //step3  区域聚类合并 矩形化

			hv_fConnectionDis = /*50*/spara1.fConnectionDis;
			connection_3d_region_by_dis_x(ho_X1, ho_Y1, ho_Z1, ho_RegionBulgeAll, &ho_RegionPartLz,&ho_RectPartLz, hv_fConnectionDis);

			//聚类再做一次合并--考虑矩形区域太近无处下刀的问题
			connection_3d_region_by_dis_x(ho_X1, ho_Y1, ho_Z1, ho_RectPartLz, &ho_RegionPartLz,&ho_RectPartLzAdv, hv_fConnectionDis);





			//***************************************************************************************************
           //step4  高度渲染


           //调平后的深度图彩色渲染
			MinMaxGray(ho_Z1PZero, ho_Z1PZero, 0, &hv_Min, &hv_Max, &hv_Range);

			hv_fRenderRangeScale = 10;
			hv_fDisMaxAbs = (hv_Max - hv_Min).TupleAbs();

			
			lCHVisionAdvXObj.scale_image_range(ho_Z1PZero, &ho_ImageScaled, -hv_fRenderRangeScale, hv_fRenderRangeScale);
			ConvertImageType(ho_ImageScaled, &ho_ImageScaleMax, "byte");
			GenImageProto(ho_ImageScaleMax, &ho_ImageClear, 255);
			TransToRgb(ho_ImageScaleMax, ho_ImageClear, ho_ImageClear, &ho_ImageR, &ho_ImageG,&ho_ImageB, "hsv");
			Compose3(ho_ImageR, ho_ImageG, ho_ImageB, &ho_Multichannel);
			ReduceDomain(ho_Multichannel, ho_Z1PZero, &ho_ImageReduced);
			CopyImage(ho_ImageReduced, &ho_Z1PZeroRender);

			lCHVisionAdvXObj.scale_image_range(ho_ImageZ1ZeroReal, &ho_ImageScaled, -hv_fRenderRangeScale,hv_fRenderRangeScale);
			ConvertImageType(ho_ImageScaled, &ho_ImageScaleMax, "byte");
			GenImageProto(ho_ImageScaleMax, &ho_ImageClear, 255);
			TransToRgb(ho_ImageScaleMax, ho_ImageClear, ho_ImageClear, &ho_ImageR, &ho_ImageG,&ho_ImageB, "hsv");
			Compose3(ho_ImageR, ho_ImageG, ho_ImageB, &ho_Multichannel);
			ReduceDomain(ho_Multichannel, ho_Z1PZero, &ho_ImageReduced);
			CopyImage(ho_ImageReduced, &ho_Z1ZeroLzRender);


			//结果数据

			spara2.bTJG = true;

			spara2.PoseTb = HTuple(hv_PoseTbC);

			if (ho_X1.IsInitialized() && ho_X1.CountObj() > 0) { spara2.X1 = ho_X1.Clone(); };
			if (ho_Y1.IsInitialized() && ho_Y1.CountObj() > 0) { spara2.Y1 = ho_Y1.Clone(); };
			if (ho_Z1.IsInitialized() && ho_Z1.CountObj() > 0) { spara2.Z1 = ho_Z1.Clone(); };



			if (ho_ImageSubZ1.IsInitialized() && ho_ImageSubZ1.CountObj() > 0) { spara2.ImageSubZ1 = ho_ImageSubZ1.Clone(); };
			if (ho_ImageZ1ZeroReal.IsInitialized() && ho_ImageZ1ZeroReal.CountObj() > 0) { spara2.ImageZ1ZeroReal = ho_ImageZ1ZeroReal.Clone(); };
			if (ho_Z1PZero.IsInitialized() && ho_Z1PZero.CountObj() > 0) { spara2.Z1PZero = ho_Z1PZero.Clone(); };
			if (ho_X1PZero.IsInitialized() && ho_X1PZero.CountObj() > 0) { spara2.X1PZero = ho_X1PZero.Clone(); };
			if (ho_Y1PZeroC.IsInitialized() && ho_Y1PZero.CountObj() > 0) { spara2.Y1PZero = ho_Y1PZero.Clone(); };

			if (ho_NzRender.IsInitialized() && ho_NzRender.CountObj() > 0) { spara2.NzRender = ho_NzRender.Clone(); };
			if (ho_Z1PZeroRender.IsInitialized() && ho_Z1PZeroRender.CountObj() > 0) { spara2.ZTbRender = ho_Z1PZeroRender.Clone(); };
			if (ho_Z1ZeroLzRender.IsInitialized() && ho_Z1ZeroLzRender.CountObj() > 0) { spara2.ZLzRender = ho_Z1ZeroLzRender.Clone(); };





			if (ho_RectPartLzAdv.IsInitialized() && ho_RectPartLzAdv.CountObj() > 0) { spara2.RectPartLz = ho_RectPartLzAdv.Clone(); };
			if (ho_RegionBulgeAll.IsInitialized() && ho_RegionBulgeAll.CountObj() > 0) { spara2.RegionBulge = ho_RegionBulgeAll.Clone(); };

			if (ho_RectangleQgSort.IsInitialized() && ho_RectangleQgSort.CountObj() > 0) { spara2.RectangleQgSort = ho_RectangleQgSort.Clone(); };

			if (ho_RegionTb.IsInitialized() && ho_RegionTb.CountObj() > 0) { spara2.RegionTb = ho_RegionTb.Clone(); };


			
			if (ho_RectangleCAdv.IsInitialized() && ho_RectangleCAdv.CountObj() > 0) { spara2.RectPartLzC = ho_RectangleCAdv.Clone(); };
			if (ho_RectangleLAdv.IsInitialized() && ho_RectangleLAdv.CountObj() > 0) { spara2.RectPartLzL = ho_RectangleLAdv.Clone(); };
			if (ho_RectangleUAdv.IsInitialized() && ho_RectangleUAdv.CountObj() > 0) { spara2.RectPartLzU = ho_RectangleUAdv.Clone(); };
			if (ho_RectangleDAdv.IsInitialized() && ho_RectangleDAdv.CountObj() > 0) { spara2.RectPartLzD = ho_RectangleDAdv.Clone(); };
			if (ho_RectangleRAdv.IsInitialized() && ho_RectangleRAdv.CountObj() > 0) { spara2.RectPartLzR = ho_RectangleRAdv.Clone(); };


			// 3D
			if (hv_ObjectModel3DAdv.Length() > 0)
			{
				try { CopyObjectModel3d(hv_ObjectModel3DAdv, "all", &spara2.OM3ImageAdv); }
				catch (HalconCpp::HException& ex) { ; }

			}

			sError.iCode = 0;
			sError.strInfo = "";

		}
		catch (HalconCpp::HException& ex)
		{
			//string str = ex.ToString();
			//CLogTxt.WriteTxt(str);

			sError.iCode = -1;
			sError.strInfo = "未知异常!";
		}


		break;
	}


	//释放所有临时变量

	CountSeconds(&time_end);
	spara2.time = time_end - time_start;
	return sError;
}


s_Rtnf CPlateB::OnLzPP(s_JggyPara spara1, s_DefectPlateBRtsPara spara0,s_PoseH PoseImgToRobot, s_LzppRtsPara& spara2)
{

	HTuple time_start = 0.0;
	HTuple time_end = 0.0;
	CountSeconds(&time_start);

	s_Rtnf sError;
	sError.iCode = -1;
	sError.strInfo = "";


	//复位结果数据
	spara2.Reset();



	// Local iconic variables 

	HObject ho_Domain, ho_X1, ho_Y1;
	HObject ho_Z1, ho_RectangleQgSort, ho_ImageSubZ1, ho_ImageZ1ZeroReal;
	HObject ho_Z1PZero, ho_RectPartLzC;
	HObject ho_X1PZero;
	HObject ho_Y1PZero;
	HObject ho_RectPartLzL, ho_RectPartLzR, ho_RectPartLzU;
	HObject ho_RectPartLzD, ho_RegionMxAllList, ho_XYZ1;
	HObject ho_RectPartLzCUnion;
	HObject ho_RegionMxRoiList, ho_RectPartLzUQgUnion;
	HObject ho_RegionMxRoiListU, ho_RectPartLzLQgUnion;
	HObject ho_RegionMxRoiListL, ho_RectPartLzDQgUnion;
	HObject ho_RegionMxRoiListD, ho_RectPartLzRQgUnion;
	HObject ho_RegionMxRoiListR, ho_ptsCross, ho_ObjectSelected;
	HObject ho_CrossSel, ho_Cross, ho_QgNotSafe;
	HObject ho_PtsQgNotSafe, ho_Arrow;
	HObject ho_ImageReduced;

	// Local control variables 

	HTuple hv_Min, hv_Max;
	HTuple hv_Range, hv_Row1;
	HTuple hv_Column1, hv_Row2;
	HTuple hv_Column2, hv_PoseTb;
	HTuple hv_HomMat3D, hv_ptsXListAll;
	HTuple hv_ptsYListAll, hv_ptsZListAll;
	HTuple hv_ptsDefListAll, hv_ptsIndexLzAll,hv_ptsRowListAll;
	HTuple hv_ptsColListAll, hv_fDjDiamiter;
	HTuple hv_ptsZMaxRelDmListAll;
	HTuple hv_ptsMxHeightCurListAll;
	HTuple hv_fJgLzUpOffset, hv_fJgDpUpOffset;
	HTuple hv_iModeJgPPZStepMode, hv_fJgPPZStepFix;
	HTuple hv_fJgPPZStepK, hv_fJgPPZStepB;
	HTuple hv_iModeJgPPYStepMode, hv_fJgPPYStepFix;
	HTuple hv_fJgPPYStepK, hv_iModeJgPPSort;
	HTuple hv_fJgPPYStepCur, hv_fYResolution;
	HTuple hv_iJgPPYStepPixelCur, hv_ptsXListZeroRoiC;
	HTuple hv_ptsYListZeroRoiC, hv_ptsZListZeroRoiC;
	HTuple hv_ptsDefListZeroRoiC, hv_ptsRowListRoiC;
	HTuple hv_ptsColListRoiC, hv_ptsIndexLzListRoiC,hv_ptsXListZeroRoiU;
	HTuple hv_ptsZMaxRelDmRoiC ;
	HTuple hv_ptsMxHeightCurRoiC ;
	HTuple hv_ptsYListZeroRoiU, hv_ptsZListZeroRoiU;
	HTuple hv_ptsDefListZeroRoiU, hv_ptsRowListRoiU;
	HTuple hv_ptsColListRoiU, hv_ptsXListZeroRoiL;
	HTuple hv_ptsYListZeroRoiL, hv_ptsZListZeroRoiL;
	HTuple hv_ptsDefListZeroRoiL, hv_ptsRowListRoiL;
	HTuple hv_ptsColListRoiL, hv_ptsXListZeroRoiD;
	HTuple hv_ptsYListZeroRoiD, hv_ptsZListZeroRoiD;
	HTuple hv_ptsDefListZeroRoiD, hv_ptsRowListRoiD;
	HTuple hv_ptsColListRoiD, hv_ptsXListZeroRoiR;
	HTuple hv_ptsYListZeroRoiR, hv_ptsZListZeroRoiR;
	HTuple hv_ptsDefListZeroRoiR, hv_ptsRowListRoiR;
	HTuple hv_ptsColListRoiR, hv_ptsXList;
	HTuple hv_ptsYList, hv_ptsZList;
	HTuple hv_fQgSafeDis, hv_fResolution;
	HTuple hv_fQgSafeDisPixel, hv_ptsQgNotSafeListAll;
	HTuple hv_iNumQg, hv_iNumPts;
	HTuple /*hv_i ,*/ hv_j , hv_IsInside;
	HTuple hv_str, hv_DistanceMin;
	HTuple hv_DistanceMax, hv_Selected;
	HTuple hv_Substrings, hv_Index;
	HTuple hv_Indices, hv_Sequence;
	HTuple hv_Reduced, hv_ptsRowListSel;
	HTuple hv_ptsColListSel, hv_Newtuple;
	HTuple hv_ptsXListAdv, hv_ptsYListAdv;
	HTuple hv_ptsZListAdv, hv_ptsDefListAdv, hv_ptsIndexLzListAdv;
	HTuple hv_ptsRowListAdv, hv_ptsColListAdv;
	HTuple hv_ptsQgNotSafeListAdv, hv_fJgTdZUp;
	HTuple hv_ptsZMaxRelDmListAdv;
	HTuple hv_ptsMxHeightCurListAdv;

	HTuple hv_Area;
	HTuple hv_Row;
	HTuple hv_Column;

	HTuple	hv_fJgPPZStepMin, hv_fJgPPZStepMax;
	
	HTuple	hv_fOffsetCutStartPos;
	HTuple	hv_fOffsetBothEndsL;
	HTuple	hv_fOffsetBothEndsR;

	HTuple hv_ptsXListRobot, hv_ptsYListRobot;
	HTuple hv_ptsZListRobot, hv_ptsDefListRobot, hv_ptsIndexLzListRobot;
	HTuple hv_ptsRowListRobot, hv_ptsColListRobot;
	HTuple hv_ptsQgNotSafeListRobot;
	HTuple hv_ptsZMaxRelDmListRobot;
	HTuple hv_ptsMxHeightCurListRobot;

	while (true)
	{
		try
		{


			//判断输入
			if (!spara0.ImageSubZ1.IsInitialized() || spara0.ImageSubZ1.CountObj() == 0)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "图像为空!";
				break;
			}



			if (spara0.PoseTb.Length() == 0)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "调平变换矩阵异常!";
				break;
			}


			if (PoseImgToRobot.IsAvailable()==false)
			{
				CountSeconds(&time_end);
				spara2.time = time_end - time_start;

				sError.iCode = 1;
				sError.strInfo = "机床变换矩阵异常!";
				break;
			}



			//输出中间变量
			hv_ptsXListAll = HTuple();
			hv_ptsYListAll = HTuple();
			hv_ptsZListAll = HTuple();
			hv_ptsDefListAll = HTuple();
			hv_ptsRowListAll = HTuple();
			hv_ptsColListAll = HTuple();

			GenEmptyObj(&ho_RegionMxAllList);


			//输入缺陷参数
			hv_PoseTb = HTuple(spara0.PoseTb);


			if (spara0.X1.IsInitialized() && spara0.X1.CountObj() > 0) { ho_X1 = spara0.X1.Clone(); };
			if (spara0.Y1.IsInitialized() && spara0.Y1.CountObj() > 0) { ho_Y1 = spara0.Y1.Clone(); };
			if (spara0.Z1.IsInitialized() && spara0.Z1.CountObj() > 0) { ho_Z1 = spara0.Z1.Clone(); };


			if (spara0.ImageSubZ1.IsInitialized() && spara0.ImageSubZ1.CountObj() > 0) { ho_ImageSubZ1 = spara0.ImageSubZ1.Clone(); };
			if (spara0.ImageZ1ZeroReal.IsInitialized() && spara0.ImageZ1ZeroReal.CountObj() > 0)
			{
				ho_ImageZ1ZeroReal = spara0.ImageZ1ZeroReal.Clone();
			};

			if (spara0.Z1PZero.IsInitialized() && spara0.Z1PZero.CountObj() > 0) { ho_Z1PZero = spara0.Z1PZero.Clone(); };
			if (spara0.X1PZero.IsInitialized() && spara0.X1PZero.CountObj() > 0) { ho_X1PZero = spara0.X1PZero.Clone(); };
			if (spara0.Y1PZero.IsInitialized() && spara0.Y1PZero.CountObj() > 0) { ho_Y1PZero = spara0.Y1PZero.Clone(); };

			if (spara0.RectPartLz.IsInitialized() && spara0.RectPartLz.CountObj() > 0) { ho_RectPartLzC = spara0.RectPartLz.Clone(); };
			if (spara0.RectangleQgSort.IsInitialized() && spara0.RectangleQgSort.CountObj() > 0) { ho_RectangleQgSort = spara0.RectangleQgSort.Clone(); };


			

			//工艺参数输入
			hv_fDjDiamiter =/* 80.0*/spara1.fDjDiamiter;
			hv_fJgLzUpOffset = /*2*/spara1.fJgLzUpOffset;
			hv_fJgDpUpOffset = /*1.5*/spara1.fJgDpUpOffset;
			hv_iModeJgPPZStepMode = /*1*/spara1.iModeJgPPZStepMode;
			hv_fJgPPZStepFix = /*2*/spara1.fJgPPZStepFix;
			hv_fJgPPZStepK = /*0.5*/spara1.fJgPPZStepK;
			hv_fJgPPZStepB = /*1.5*/spara1.fJgPPZStepB;
			hv_fJgPPZStepMin = /*3.0*/spara1.fJgPPZStepMin; //最小最大磨削量 只针对线性进给
			hv_fJgPPZStepMax = /*8.0*/spara1.fJgPPZStepMax;

			hv_iModeJgPPYStepMode = /*0*/spara1.iModeJgPPYStepMode;
			hv_fJgPPYStepFix = /*70*/spara1.fJgPPYStepFix;
			hv_fJgPPYStepK = /*0.75*/spara1.fJgPPYStepK;

			hv_iModeJgPPSort = /*0*/spara1.iModeJgPPSort; 
			hv_fQgSafeDis =/* 60.0*/spara1.fQgSafeDis;
			hv_fJgTdZUp = /*20*/spara1.fJgTdZUp;


	

			hv_fOffsetCutStartPos = /*50*/spara1.fOffsetCutStartPos;
			hv_fOffsetBothEndsL = /*10*/spara1.fOffsetBothEndsL;
			hv_fOffsetBothEndsR = /*10*/spara1.fOffsetBothEndsR;



			//计算当前进给Y
			hv_fJgPPYStepCur = 20;
			if (0 != (int(hv_iModeJgPPYStepMode == 0)))
			{
				hv_fJgPPYStepCur = hv_fJgPPYStepFix;
			}
			else
			{
				hv_fJgPPYStepCur = hv_fJgPPYStepK * hv_fDjDiamiter;
			}

		
			

			hv_ptsXListZeroRoiC = HTuple();
			hv_ptsYListZeroRoiC = HTuple();
			hv_ptsZListZeroRoiC = HTuple();
			hv_ptsDefListZeroRoiC = HTuple();
			hv_ptsRowListRoiC = HTuple();
			hv_ptsColListRoiC = HTuple();

			hv_ptsZMaxRelDmRoiC = HTuple();
		    hv_ptsMxHeightCurRoiC = HTuple();

			hv_ptsIndexLzListRoiC== HTuple();


			GenEmptyObj(&ho_RegionMxRoiList);
			cal_lzgj_image_pp_r1_x(ho_X1PZero, ho_Y1PZero, ho_Z1PZero, ho_ImageZ1ZeroReal,
				ho_ImageSubZ1, spara0.RectPartLz, spara0.RegionTb, &ho_RegionMxRoiList, hv_iModeJgPPSort,
				hv_fJgPPYStepCur, hv_iModeJgPPZStepMode, hv_fJgPPZStepFix, hv_fJgPPZStepK,
				hv_fJgPPZStepB, hv_fJgPPZStepMin, hv_fJgPPZStepMax, hv_fJgLzUpOffset, hv_fJgDpUpOffset,
				hv_fOffsetCutStartPos, hv_fOffsetBothEndsL, hv_fOffsetBothEndsR, hv_fJgTdZUp,
				&hv_ptsXListZeroRoiC, &hv_ptsYListZeroRoiC, &hv_ptsZListZeroRoiC, &hv_ptsDefListZeroRoiC,
				&hv_ptsRowListRoiC, &hv_ptsColListRoiC,&hv_ptsZMaxRelDmRoiC, &hv_ptsMxHeightCurRoiC, &hv_ptsIndexLzListRoiC);




			int n1 = hv_ptsColListRoiC.Length();

			int n2 = hv_ptsMxHeightCurRoiC.Length();
			int n3 = hv_ptsZMaxRelDmRoiC.Length();

			std::vector<double> mmm;
			std::vector<double> ddd;

			for (int i = 0; i < hv_ptsMxHeightCurRoiC.Length(); i++)
			{
				mmm.push_back(hv_ptsZMaxRelDmRoiC[i]);
				ddd.push_back(hv_ptsMxHeightCurRoiC[i]);
			}
			


			ConcatObj(ho_RegionMxAllList, ho_RegionMxRoiList, &ho_RegionMxAllList);


			int iNumPts = hv_ptsXListZeroRoiC.Length();

			//转换成实际坐标系下的坐标
			PoseToHomMat3d(hv_PoseTb, &hv_HomMat3D);

			AffineTransPoint3d(hv_HomMat3D, hv_ptsXListZeroRoiC, hv_ptsYListZeroRoiC,
				hv_ptsZListZeroRoiC, &hv_ptsXList, &hv_ptsYList, &hv_ptsZList);
			hv_ptsXListAll = hv_ptsXListAll.TupleConcat(hv_ptsXList);
			hv_ptsYListAll = hv_ptsYListAll.TupleConcat(hv_ptsYList);
			hv_ptsZListAll = hv_ptsZListAll.TupleConcat(hv_ptsZList);
			hv_ptsDefListAll = hv_ptsDefListAll.TupleConcat(hv_ptsDefListZeroRoiC);
			hv_ptsIndexLzAll = hv_ptsIndexLzAll.TupleConcat(hv_ptsIndexLzListRoiC);

			hv_ptsRowListAll = hv_ptsRowListAll.TupleConcat(hv_ptsRowListRoiC);
			hv_ptsColListAll = hv_ptsColListAll.TupleConcat(hv_ptsColListRoiC);

			hv_ptsZMaxRelDmListAll = hv_ptsZMaxRelDmListAll.TupleConcat(hv_ptsZMaxRelDmRoiC);
			hv_ptsMxHeightCurListAll = hv_ptsMxHeightCurListAll.TupleConcat(hv_ptsMxHeightCurRoiC);

			
			/*int */iNumPts = hv_ptsColListAll.Length();

			//气缸避位处理

			GenCrossContourXld(&ho_ptsCross, hv_ptsRowListAll, hv_ptsColListAll, 11, 0);


			//计算像素分辨率

			ReduceDomain(ho_X1, ho_RectPartLzC, &ho_ImageReduced);
			GetDomain(ho_ImageReduced, &ho_Domain);
			MinMaxGray(ho_Domain, ho_X1, 0, &hv_Min, &hv_Max, &hv_Range);
			SmallestRectangle1(ho_Domain, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
			hv_fResolution = hv_Range / (((hv_Column1 - hv_Column2)).TupleAbs());
			hv_fQgSafeDisPixel = hv_fQgSafeDis / hv_fResolution;
			TupleGenConst(hv_ptsDefListAll.TupleLength(), "", &hv_ptsQgNotSafeListAll);


			CountObj(ho_RectangleQgSort, &hv_iNumQg);
			hv_iNumPts = hv_ptsRowListAll.TupleLength();


			for (int iPts = 0; iPts < hv_iNumPts.I(); iPts++)
			{
				for (int iQg = 0; iQg < hv_iNumQg.I(); iQg++)
				{
					SelectObj(ho_RectangleQgSort, &ho_ObjectSelected, iQg + 1);

					//气缸不存在的时候 直接跳过
					AreaCenter(ho_ObjectSelected, &hv_Area, &hv_Row, &hv_Column);
					if (0 != (int(hv_Area < 100)))
					{
						continue;
					}

					//测试包含情况
					SmallestRectangle1(ho_ObjectSelected, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
					TestRegionPoint(ho_ObjectSelected, hv_ptsRowListAll.TupleSelect(iPts), hv_ptsColListAll.TupleSelect(iPts), &hv_IsInside);


					if (0 != (int(hv_IsInside == 1)))
					{


						if (0 != (int(HTuple(hv_ptsQgNotSafeListAll[iPts]) == HTuple(""))))
						{
							hv_ptsQgNotSafeListAll[iPts] = HTuple(iQg).TupleString("d");

						}
						else
						{
							hv_str = (HTuple(hv_ptsQgNotSafeListAll[iPts]) + HTuple(",")) + (HTuple(iQg).TupleString("d"));
							hv_ptsQgNotSafeListAll[iPts] = hv_str;

						}


						continue;

					}



					//测试当前点到气缸区域的距离
					DistancePr(ho_ObjectSelected, HTuple(hv_ptsRowListAll[iPts]), HTuple(hv_ptsColListAll[iPts]),
						&hv_DistanceMin, &hv_DistanceMax);



					if (0 != (int(hv_DistanceMin < hv_fQgSafeDisPixel)))
					{

						if (0 != (int(HTuple(hv_ptsQgNotSafeListAll[iPts]) == HTuple(""))))
						{

							hv_ptsQgNotSafeListAll[iPts] = HTuple(iQg).TupleString("d");
						}
						else
						{
							hv_str = (HTuple(hv_ptsQgNotSafeListAll[iPts]) + HTuple(",")) + (HTuple(iQg).TupleString("d"));
							hv_ptsQgNotSafeListAll[iPts] = hv_str;
						}

						continue;
					}


					//测试从上一个路径点运动到当前点 直线段的距离
					if (iPts>0)
					{
						DistanceSr(ho_ObjectSelected, HTuple(hv_ptsRowListAll[iPts-1]), HTuple(hv_ptsColListAll[iPts-1]),
							HTuple(hv_ptsRowListAll[iPts]), HTuple(hv_ptsColListAll[iPts]), &hv_DistanceMin,
							&hv_DistanceMax);

						if (0 != (int(hv_DistanceMin < hv_fQgSafeDisPixel)))
						{

							if (0 != (int(HTuple(hv_ptsQgNotSafeListAll[iPts]) == HTuple(""))))
							{
								hv_ptsQgNotSafeListAll[iPts] = HTuple(iQg).TupleString("d");
							}
							else
							{
								hv_str = (HTuple(hv_ptsQgNotSafeListAll[iPts]) + HTuple(",")) + (HTuple(iQg).TupleString("d"));
								hv_ptsQgNotSafeListAll[iPts] = hv_str;
							}


							continue;
						
						}

					}


				}

			}


			//****************************************************************************
          //点属性定义//1 起点  2 终点  3 中间点  4空跑点
		//对路径点进行修改
		//起点+中间点+中间点/中点 且起点和终在一条直线上 去除掉一个中间点


			hv_ptsXListAdv = HTuple();
			hv_ptsYListAdv = HTuple();
			hv_ptsZListAdv = HTuple();
			hv_ptsDefListAdv = HTuple();
			hv_ptsRowListAdv = HTuple();
			hv_ptsColListAdv = HTuple();
			hv_ptsQgNotSafeListAdv = HTuple();

			hv_ptsZMaxRelDmListAdv = HTuple();
			hv_ptsMxHeightCurListAdv = HTuple();

			

			//hv_ptsXListAdv = hv_ptsXListAll;
			//hv_ptsYListAdv = hv_ptsYListAll;
			//hv_ptsZListAdv = hv_ptsZListAll;
			//hv_ptsDefListAdv = hv_ptsDefListAll;
			//hv_ptsRowListAdv = hv_ptsRowListAll;
			//hv_ptsColListAdv = hv_ptsColListAll;
			//hv_ptsQgNotSafeListAdv = hv_ptsQgNotSafeListAll;
			//hv_ptsZMaxRelDmListAdv = hv_ptsZMaxRelDmListAll;
			//hv_ptsMxHeightCurListAdv = hv_ptsMxHeightCurListAll;



			hv_Indices = HTuple();
			{
				HTuple end_val1560 = (hv_ptsDefListAll.TupleLength()) - 3;
				HTuple step_val1560 = 1;
				for (hv_j = 0; hv_j.Continue(end_val1560, step_val1560); hv_j += step_val1560)
				{
					if (0 != (HTuple(HTuple(int(HTuple(hv_ptsDefListAll[hv_j]) == 1)).TupleAnd(int(HTuple(hv_ptsDefListAll[hv_j + 1]) == 3))).TupleAnd(HTuple(int(HTuple(hv_ptsDefListAll[hv_j + 2]) == 3)).TupleOr(int(HTuple(hv_ptsDefListAll[hv_j + 2]) == 2)))))
					{
						if (0 != (HTuple(int(((HTuple(hv_ptsYListAll[hv_j]) - HTuple(hv_ptsYListAll[hv_j + 1])).TupleAbs()) < 5)).TupleAnd(int(((HTuple(hv_ptsYListAll[hv_j + 1]) - HTuple(hv_ptsYListAll[hv_j + 2])).TupleAbs()) < 5))))
						{
							hv_Indices = hv_Indices.TupleConcat(hv_j + 1);
						}
					}
				}
			}

			TupleRemove(hv_ptsXListAll, hv_Indices, &hv_ptsXListAdv);
			TupleRemove(hv_ptsYListAll, hv_Indices, &hv_ptsYListAdv);
			TupleRemove(hv_ptsZListAll, hv_Indices, &hv_ptsZListAdv);
			TupleRemove(hv_ptsDefListAll, hv_Indices, &hv_ptsDefListAdv);
			TupleRemove(hv_ptsIndexLzAll, hv_Indices, &hv_ptsIndexLzListAdv);
			TupleRemove(hv_ptsRowListAll, hv_Indices, &hv_ptsRowListAdv);
			TupleRemove(hv_ptsColListAll, hv_Indices, &hv_ptsColListAdv);
			TupleRemove(hv_ptsQgNotSafeListAll, hv_Indices, &hv_ptsQgNotSafeListAdv);
			TupleRemove(hv_ptsZMaxRelDmListAll, hv_Indices, &hv_ptsZMaxRelDmListAdv);
			TupleRemove(hv_ptsMxHeightCurListAll, hv_Indices, &hv_ptsMxHeightCurListAdv);
			







			iNumPts = hv_ptsXListAdv.Length();

			//****************************************************************************
			//将坐标转换成机械手坐标系下的坐标
			hv_ptsXListRobot = HTuple();
			hv_ptsYListRobot = HTuple();
			hv_ptsZListRobot = HTuple();
			hv_ptsDefListRobot = HTuple();
			hv_ptsIndexLzListRobot = HTuple();
			hv_ptsRowListRobot = HTuple();
			hv_ptsColListRobot = HTuple();
			hv_ptsQgNotSafeListRobot = HTuple();

			hv_ptsZMaxRelDmListRobot  = HTuple();
			hv_ptsMxHeightCurListRobot = HTuple();

			

			hv_ptsXListRobot = hv_ptsXListAdv;
			hv_ptsYListRobot = hv_ptsYListAdv;
			hv_ptsZListRobot = hv_ptsZListAdv;
			hv_ptsDefListRobot = hv_ptsDefListAdv;
			hv_ptsIndexLzListRobot = hv_ptsIndexLzListAdv;

			hv_ptsRowListRobot = hv_ptsRowListAdv;
			hv_ptsColListRobot = hv_ptsColListAdv;
			hv_ptsQgNotSafeListRobot = hv_ptsQgNotSafeListAdv;

			hv_ptsZMaxRelDmListRobot = hv_ptsZMaxRelDmListAdv;
			hv_ptsMxHeightCurListRobot = hv_ptsMxHeightCurListAdv;

	
			PoseToHomMat3d(PoseImgToRobot.ToTuple(), &hv_HomMat3D);
			AffineTransPoint3d(hv_HomMat3D, hv_ptsXListAdv, hv_ptsYListAdv, hv_ptsZListAdv, &hv_ptsXListRobot, &hv_ptsYListRobot, &hv_ptsZListRobot);


			//两个空跑点连续时候  修改一下Z坐标 让空跑高度一致
			for (size_t i = 0; i < hv_ptsZListRobot.Length() - 1; i++)
			{
				if (hv_ptsDefListRobot[i] == 4 && hv_ptsDefListRobot[i + 1] == 4 && hv_ptsIndexLzListRobot[i] != hv_ptsIndexLzListRobot[i + 1])
				{
					if (hv_ptsZListRobot[i].D() <hv_ptsZListRobot[i + 1].D())
					{
						hv_ptsZListRobot[i] = hv_ptsZListRobot[i + 1].D();
					}

				}

			}


	
			//****************************************************************************
            //机床坐标系下补偿
			hv_ptsXListRobot += spara1.fOffsetPtsX;
			hv_ptsYListRobot += spara1.fOffsetPtsY;
			hv_ptsZListRobot += spara1.fOffsetPtsZ;

			/*int */iNumPts = hv_ptsXListRobot.Length();

			//结果数据

			spara2.bTJG = true;


			spara2.sListPPtsImage.clear();
			for (int i = 0; i < hv_ptsDefListAdv.Length(); i++)
			{
				s_PPts pt;

				pt.fX = hv_ptsXListAdv[i].D();
				pt.fY = hv_ptsYListAdv[i].D();
				pt.fZ = hv_ptsZListAdv[i].D();
				pt.iDef = hv_ptsDefListAdv[i].I();
				pt.indexLz= hv_ptsIndexLzListAdv[i].I();
				
				pt.fRow = hv_ptsRowListAdv[i].D();
				pt.fCol = hv_ptsColListAdv[i].D();
				pt.strQgNotSafe = hv_ptsQgNotSafeListAdv[i].S();

				pt.ZMaxRelDm= hv_ptsZMaxRelDmListAdv[i].D();
				pt.MxHeightCur = hv_ptsMxHeightCurListAdv[i].D();

				spara2.sListPPtsImage.push_back(pt);
			}

			//spara2.iNumPts = hv_ptsDefListAdv.Length();




			spara2.sListPPtsRobot.clear();
			for (int i = 0; i < hv_ptsDefListRobot.Length(); i++)
			{
				s_PPts pt;

				pt.fX = hv_ptsXListRobot[i].D();
				pt.fY = hv_ptsYListRobot[i].D();
				pt.fZ = hv_ptsZListRobot[i].D();
				pt.iDef = hv_ptsDefListRobot[i].I();
				pt.indexLz = hv_ptsIndexLzListRobot[i].I();
				pt.fRow = hv_ptsRowListRobot[i].D();
				pt.fCol = hv_ptsColListRobot[i].D();
				pt.strQgNotSafe = hv_ptsQgNotSafeListRobot[i].S();

				pt.ZMaxRelDm = hv_ptsZMaxRelDmListRobot[i].D();
				pt.MxHeightCur = hv_ptsMxHeightCurListRobot[i].D();

				spara2.sListPPtsRobot.push_back(pt);
			}

			spara2.iNumPts = hv_ptsDefListRobot.Length();



			int iNumRegion = ho_RegionMxAllList.CountObj();

			spara2.RegionMxAllList.Clear();
			if (ho_RegionMxAllList.IsInitialized() && ho_RegionMxAllList.CountObj() > 0) { spara2.RegionMxAllList = ho_RegionMxAllList.Clone(); };


			sError.iCode = 0;
			sError.strInfo = "";

		}
		catch (HalconCpp::HException& ex)
		{
			/*            string str = ex.ToString();
						CLogTxt.WriteTxt(str);*/

			sError.iCode = -1;
			sError.strInfo = "未知异常!";
		}


		break;
	}


	//释放所有临时变量

	CountSeconds(&time_end);
	spara2.time = time_end - time_start;
	return sError;
}


bool CPlateB::CalcRtMat(std::vector <double> fListX1, std::vector <double> fListY1, std::vector <double> fListZ1,
	std::vector <double> fListX2, std::vector <double> fListY2, std::vector <double> fListZ2,
	s_PoseH &PoseT)
{

	if (fListX1.size() < 4)
	{
		return false;
	}

	if (fListX1.size() != fListX2.size())
	{
		return false;
	}


	try
	{

		HTuple hv_Px, hv_Py, hv_Pz, hv_Qx, hv_Qy, hv_Qz;

		hv_Px = HTuple();
		for (int i = 0; i < fListX1.size(); i++)
		{
			hv_Px.Append(fListX1[i]);
		}

		hv_Py = HTuple();
		for (int i = 0; i < fListY1.size(); i++)
		{
			hv_Py.Append(fListY1[i]);
		}

		hv_Pz = HTuple();
		for (int i = 0; i < fListZ1.size(); i++)
		{
			hv_Pz.Append(fListZ1[i]);
		}


		hv_Qx = HTuple();
		for (int i = 0; i < fListX2.size(); i++)
		{
			hv_Qx.Append(fListX2[i]);
		}

		hv_Qy = HTuple();
		for (int i = 0; i < fListY2.size(); i++)
		{
			hv_Qy.Append(fListY2[i]);
		}

		hv_Qz = HTuple();
		for (int i = 0; i < fListZ2.size(); i++)
		{
			hv_Qz.Append(fListZ2[i]);
		}


		HTuple hv_HomMat3D, hv_Pose;

		VectorToHomMat3d("similarity", hv_Px, hv_Py, hv_Pz, hv_Qx, hv_Qy, hv_Qz, &hv_HomMat3D);
		HomMat3dToPose(hv_HomMat3D, &hv_Pose);

		PoseT = s_PoseH(hv_Pose);
		



	}
	catch (HalconCpp::HException& ex)
	{
		//string str = ex.ToString();
		//CLogTxt.WriteTxt(str);

		return false;
	}

	return true;
}


void CPlateB::get_plane_ranger_z_out_region_x(HObject ho_Z, HObject* ho_RegionD, HObject* ho_ZSub,
	HObject* ho_ZZeroReal, HTuple hv_ThZLower, HTuple hv_ThZHiger, HTuple hv_ThZLimit,
	HTuple hv_MaskWidth, HTuple hv_MaskHeight, HTuple hv_Expand)
{

	// Local iconic variables
	HObject  ho_Domain, ho_RegionFillUp, ho_RegionInnerLess;
	HObject  ho_ZaFill, ho_RegionOuterLess, ho_RegionInner, ho_Zb;
	HObject  ho_Z1PZeroEx, ho_ImageMean, ho_Region1, ho_Region2;
	HObject  ho_ConnectedRegions, ho_SelectedRegions, ho_ObjectSelected;
	HObject  ho_RegionIntersection, ho_ImageMeanZ;

	// Local control variables
	HTuple  hv_Rows, hv_Columns, hv_Newtuple, hv_Number;
	HTuple  hv_I, hv_Area, hv_Row, hv_Column, hv_Grayval;



	//先对内部缺失点进行填充

	GetDomain(ho_Z, &ho_Domain);
	FillUp(ho_Domain, &ho_RegionFillUp);
	Difference(ho_RegionFillUp, ho_Domain, &ho_RegionInnerLess);
	GenImageProto(ho_Z, &ho_ZaFill, 0);
	OverpaintGray(ho_ZaFill, ho_Z);
	Difference(ho_ZaFill, ho_RegionFillUp, &ho_RegionOuterLess);
	Difference(ho_ZaFill, ho_RegionOuterLess, &ho_RegionInner);

	GetRegionPoints(ho_RegionInnerLess, &hv_Rows, &hv_Columns);
	TupleGenConst(hv_Rows.TupleLength(), 0, &hv_Newtuple);
	SetGrayval(ho_ZaFill, hv_Rows, hv_Columns, hv_Newtuple);
	ReduceDomain(ho_ZaFill, ho_RegionInner, &ho_Zb);


	//dev_clear_window ()
	//dev_display (Zb)
	//stop ()



	ExpandDomainGray(ho_Zb, &ho_Z1PZeroEx, hv_Expand);
	MeanImage(ho_Z1PZeroEx, &ho_ImageMean, hv_MaskWidth, hv_MaskHeight);
	SubImage(ho_ImageMean, ho_Z1PZeroEx, &(*ho_ZSub), 1, 0);

	GetDomain(ho_Z, &ho_Domain);
	FillUp(ho_Domain, &ho_RegionFillUp);
	ReduceDomain((*ho_ZSub), ho_RegionFillUp, &(*ho_ZSub));



	Threshold((*ho_ZSub), &ho_Region1, hv_ThZLower, hv_ThZLimit);
	Threshold((*ho_ZSub), &ho_Region2, hv_ThZHiger, hv_ThZLimit);

	Connection(ho_Region2, &ho_ConnectedRegions);
	SelectShape(ho_ConnectedRegions, &ho_SelectedRegions, "area", "and", 2, 999999);
	SelectShape(ho_SelectedRegions, &ho_SelectedRegions, "width", "and", 2, 999999);
	SelectShape(ho_SelectedRegions, &ho_SelectedRegions, "height", "and", 2, 999999);
	Union1(ho_SelectedRegions, &ho_Region2);

	//dev_clear_window ()
	//dev_display (Z)
	//dev_display (SelectedRegions)


	Connection(ho_Region1, &ho_ConnectedRegions);

	GenEmptyObj(&(*ho_RegionD));

	CountObj(ho_ConnectedRegions, &hv_Number);
	{
		HTuple end_val53 = hv_Number;
		HTuple step_val53 = 1;
		for (hv_I = 1; hv_I.Continue(end_val53, step_val53); hv_I += step_val53)
		{
			SelectObj(ho_ConnectedRegions, &ho_ObjectSelected, hv_I);
			Intersection(ho_ObjectSelected, ho_Region2, &ho_RegionIntersection);
			AreaCenter(ho_RegionIntersection, &hv_Area, &hv_Row, &hv_Column);
			if (0 != (int(hv_Area > 0)))
			{
				ConcatObj((*ho_RegionD), ho_ObjectSelected, &(*ho_RegionD));
			}

		}
	}



	SelectShape((*ho_RegionD), &ho_SelectedRegions, "area", "and", 9, 999999);
	SelectShape(ho_SelectedRegions, &ho_SelectedRegions, "inner_width", "and", 3, 999999);
	SelectShape(ho_SelectedRegions, &(*ho_RegionD), "inner_height", "and", 3, 999999);




	//dev_clear_window ()
	//dev_display (Z)
	//dev_display (RegionD)
	//stop ()

	//额外输出一份真实的高度 Z向上为正

	MeanImage(ho_Z, &ho_ImageMeanZ, 3, 3);
	CopyImage((*ho_ZSub), &(*ho_ZZeroReal));
	CountObj((*ho_RegionD), &hv_Number);
	{
		HTuple end_val82 = hv_Number;
		HTuple step_val82 = 1;
		for (hv_I = 1; hv_I.Continue(end_val82, step_val82); hv_I += step_val82)
		{

			SelectObj(ho_ConnectedRegions, &ho_ObjectSelected, hv_I);
			GetDomain(ho_ObjectSelected, &ho_Domain);
			GetRegionPoints(ho_Domain, &hv_Row, &hv_Column);
			GetGrayval(ho_ImageMeanZ, hv_Row, hv_Column, &hv_Grayval);
			SetGrayval((*ho_ZZeroReal), hv_Row, hv_Column, -hv_Grayval);

		}
	}


	return;
}




//void CPlateB::get_plane_ranger_z_out_region_x(HObject ho_Z, HObject* ho_RegionD, HObject* ho_ZSub,
//	HObject* ho_ZZeroReal, HTuple hv_ThZLower, HTuple hv_ThZHiger, HTuple hv_ThZLimit,
//	HTuple hv_MaskWidth, HTuple hv_MaskHeight, HTuple hv_Expand)
//{
//
//	// Local iconic variables
//	HObject  ho_Domain, ho_RegionFillUp, ho_RegionInnerLess;
//	HObject  ho_ZaFill, ho_RegionOuterLess, ho_RegionInner, ho_Zb;
//	HObject  ho_Z1PZeroEx, ho_ImageMean, ho_Region1, ho_Region2;
//	HObject  ho_ConnectedRegions, ho_ObjectSelected, ho_RegionIntersection;
//	HObject  ho_ImageMeanZ;
//
//	// Local control variables
//	HTuple  hv_Rows, hv_Columns, hv_Newtuple, hv_Number;
//	HTuple  hv_I, hv_Area, hv_Row, hv_Column, hv_Grayval;
//
//
//
//	//先对内部缺失点进行填充
//
//	GetDomain(ho_Z, &ho_Domain);
//	FillUp(ho_Domain, &ho_RegionFillUp);
//	Difference(ho_RegionFillUp, ho_Domain, &ho_RegionInnerLess);
//	GenImageProto(ho_Z, &ho_ZaFill, 0);
//	OverpaintGray(ho_ZaFill, ho_Z);
//	Difference(ho_ZaFill, ho_RegionFillUp, &ho_RegionOuterLess);
//	Difference(ho_ZaFill, ho_RegionOuterLess, &ho_RegionInner);
//
//	GetRegionPoints(ho_RegionInnerLess, &hv_Rows, &hv_Columns);
//	TupleGenConst(hv_Rows.TupleLength(), 0, &hv_Newtuple);
//	SetGrayval(ho_ZaFill, hv_Rows, hv_Columns, hv_Newtuple);
//	ReduceDomain(ho_ZaFill, ho_RegionInner, &ho_Zb);
//
//
//	//dev_clear_window ()
//	//dev_display (Zb)
//	//stop ()
//
//
//
//	ExpandDomainGray(ho_Zb, &ho_Z1PZeroEx, hv_Expand);
//	MeanImage(ho_Z1PZeroEx, &ho_ImageMean, hv_MaskWidth, hv_MaskHeight);
//	SubImage(ho_ImageMean, ho_Z1PZeroEx, &(*ho_ZSub), 1, 0);
//
//	GetDomain(ho_Z, &ho_Domain);
//	FillUp(ho_Domain, &ho_RegionFillUp);
//	ReduceDomain((*ho_ZSub), ho_RegionFillUp, &(*ho_ZSub));
//
//
//
//	Threshold((*ho_ZSub), &ho_Region1, hv_ThZLower, hv_ThZLimit);
//	Threshold((*ho_ZSub), &ho_Region2, hv_ThZHiger, hv_ThZLimit);
//	Connection(ho_Region1, &ho_ConnectedRegions);
//
//	GenEmptyObj(&(*ho_RegionD));
//
//	CountObj(ho_ConnectedRegions, &hv_Number);
//	{
//		HTuple end_val41 = hv_Number;
//		HTuple step_val41 = 1;
//		for (hv_I = 1; hv_I.Continue(end_val41, step_val41); hv_I += step_val41)
//		{
//			SelectObj(ho_ConnectedRegions, &ho_ObjectSelected, hv_I);
//			Intersection(ho_ObjectSelected, ho_Region2, &ho_RegionIntersection);
//			AreaCenter(ho_RegionIntersection, &hv_Area, &hv_Row, &hv_Column);
//			if (0 != (int(hv_Area > 0)))
//			{
//				ConcatObj((*ho_RegionD), ho_ObjectSelected, &(*ho_RegionD));
//			}
//
//		}
//	}
//
//
//	//dev_clear_window ()
//	//dev_display (Z)
//	//dev_display (RegionD)
//	//stop ()
//
//	//额外输出一份真实的高度 Z向上为正
//
//	MeanImage(ho_Z, &ho_ImageMeanZ, 3, 3);
//	CopyImage((*ho_ZSub), &(*ho_ZZeroReal));
//	CountObj((*ho_RegionD), &hv_Number);
//	{
//		HTuple end_val62 = hv_Number;
//		HTuple step_val62 = 1;
//		for (hv_I = 1; hv_I.Continue(end_val62, step_val62); hv_I += step_val62)
//		{
//			SelectObj(ho_ConnectedRegions, &ho_ObjectSelected, hv_I);
//			GetDomain(ho_ObjectSelected, &ho_Domain);
//			GetRegionPoints(ho_Domain, &hv_Row, &hv_Column);
//			GetGrayval(ho_ImageMeanZ, hv_Row, hv_Column, &hv_Grayval);
//			SetGrayval((*ho_ZZeroReal), hv_Row, hv_Column, -hv_Grayval);
//
//		}
//	}
//
//
//	return;
//}




void CPlateB::get_qg_z_out_region_x(HObject ho_Z, HObject ho_RegionQg, HObject* ho_ZSub,
	HObject* ho_RegionD, HObject* ho_ZZeroReal, HTuple hv_QgHOrg, HTuple hv_ThZOuter,
	HTuple hv_ThZlimit)
{

	// Local iconic variables
	HObject  ho_RegionFillUp, ho_ConnectedRegions;
	HObject  ho_ObjectSelected, ho_RegionDilation, ho_RegionDifference;
	HObject  ho_Domain, ho_RegionIntersection, ho_Region;

	// Local control variables
	HTuple  hv_Width, hv_Height, hv_Number, hv_I;
	HTuple  hv_MeanRef, hv_Deviation, hv_MeanMark, hv_HQg, hv_HQgLz;
	HTuple  hv_Rows, hv_Columns, hv_Newtuple, hv_HQg2, hv_HQgLz2;
	HTuple  hv_Newtuple2;




	GetImageSize(ho_Z, &hv_Width, &hv_Height);
	GenImageConst(&(*ho_ZSub), "real", hv_Width, hv_Height);
	GenImageConst(&(*ho_ZZeroReal), "real", hv_Width, hv_Height);
	GenEmptyObj(&(*ho_RegionD));



	FillUp(ho_RegionQg, &ho_RegionFillUp);

	//dev_clear_window ()
	//dev_display (Z)
	//dev_set_draw ('fill')
	//dev_display (RegionQg)

	Connection(ho_RegionFillUp, &ho_ConnectedRegions);
	CountObj(ho_ConnectedRegions, &hv_Number);
	{
		HTuple end_val19 = hv_Number;
		HTuple step_val19 = 1;
		for (hv_I = 1; hv_I.Continue(end_val19, step_val19); hv_I += step_val19)
		{

			//dev_clear_window ()
			//dev_display (Z)

			SelectObj(ho_ConnectedRegions, &ho_ObjectSelected, hv_I);


			DilationRectangle1(ho_ObjectSelected, &ho_RegionDilation, 71, 71);
			Difference(ho_RegionDilation, ho_ObjectSelected, &ho_RegionDifference);

			GetDomain(ho_Z, &ho_Domain);
			Intersection(ho_RegionDifference, ho_Domain, &ho_RegionIntersection);
			Intensity(ho_RegionIntersection, ho_Z, &hv_MeanRef, &hv_Deviation);


			Intersection(ho_ObjectSelected, ho_Z, &ho_RegionIntersection);
			Threshold(ho_Z, &ho_Region, -100, 20);

			Intersection(ho_RegionIntersection, ho_Region, &ho_RegionIntersection);

			//dev_clear_window ()
			//dev_display (Z)
			//dev_display (RegionIntersection)


			Intensity(ho_RegionIntersection, ho_Z, &hv_MeanMark, &hv_Deviation);

			hv_HQg = (hv_MeanMark - hv_MeanRef).TupleAbs();
			hv_HQgLz = hv_HQg - hv_QgHOrg;

			GetRegionPoints(ho_ObjectSelected, &hv_Rows, &hv_Columns);
			TupleGenConst(hv_Rows.TupleLength(), hv_HQgLz, &hv_Newtuple);
			SetGrayval((*ho_ZSub), hv_Rows, hv_Columns, hv_Newtuple);


			hv_HQg2 = hv_MeanMark.TupleAbs();
			hv_HQgLz2 = hv_HQg2 - hv_QgHOrg;
			TupleGenConst(hv_Rows.TupleLength(), hv_HQgLz2, &hv_Newtuple2);
			SetGrayval((*ho_ZZeroReal), hv_Rows, hv_Columns, hv_Newtuple2);

			if (0 != (HTuple(int(hv_HQgLz > hv_ThZOuter)).TupleAnd(int(hv_HQgLz < hv_ThZlimit))))
			{

				ConcatObj((*ho_RegionD), ho_ObjectSelected, &(*ho_RegionD));
			}


		}
	}

	return;
}



// Local procedures 
void CPlateB::cal_lzgj_image_pp_r1_x(HObject ho_X, HObject ho_Y, HObject ho_Z, HObject ho_ZReal,
	HObject ho_ZSub, HObject ho_RectLzs, HObject ho_RegionTb, HObject* ho_RegionMXs,
	HTuple hv_iModeJgPPSort, HTuple hv_fJgPPYStep, HTuple hv_iModeJgPPZStepMode,
	HTuple hv_fJgPPZStepFix, HTuple hv_fJgPPZStepK, HTuple hv_fJgPPZStepB, HTuple hv_fJgPPZStepMin,
	HTuple hv_fJgPPZStepMax, HTuple hv_fJgLzUpOffset, HTuple hv_fJgDpUpOffset, HTuple hv_fOffsetCutStartPos,
	HTuple hv_fOffsetBothEndsL, HTuple hv_fOffsetBothEndsR, HTuple hv_fJgTdZUp, HTuple* hv_ptsXList,
	HTuple* hv_ptsYList, HTuple* hv_ptsZList, HTuple* hv_ptsDefList, HTuple* hv_ptsRowList,
	HTuple* hv_ptsColList, HTuple* hv_ptsZMaxRelDmList, HTuple* hv_ptsMxHeightCurList, HTuple* hv_ptsIndexLzList)
{

	// Local iconic variables
	HObject  ho_SortedRegions, ho_RectangleU, ho_RectangleD;
	HObject  ho_RectangleL, ho_RectangleR, ho_RectangleC, ho_LzSelected;
	HObject  ho_RegionIntersection, ho_RegionDifference, ho_Partitioned;
	HObject  ho_RegionMxList, ho_ObjectSelected, ho_Domain, ho_RegionMx;
	HObject  ho_Cross;

	HObject   ho_RegionRemain;
	HObject   ho_SortedRegions0, ho_SortedRegions1;
	HObject  ho_SortedRegions2, ho_SortedRegions3, ho_SortedRegions4;
	HObject  ho_Contour;


	// Local control variables
	HTuple  hv_Min, hv_Max, hv_Range, hv_Row1, hv_Column1;
	HTuple  hv_Row2, hv_Column2, hv_fYResolution, hv_iJgPPYStepPixelCur;
	HTuple  hv_NumLz, hv_iNumLz, hv_ddd, hv_iCutDir, hv_AreaU;
	HTuple  hv_Row, hv_Column, hv_AreaD, hv_AreaR, hv_AreaL;
	HTuple  hv_AreaC, hv_NumPart, hv_ptsXListZeroS, hv_ptsYListZeroS;
	HTuple  hv_ptsZListZeroS, hv_ptsDefListZeroS, hv_ptsRowListS;
	HTuple  hv_ptsColListS, hv_bOdd, hv_iPart, hv_MinX, hv_MaxX;
	HTuple  hv_MinY, hv_MaxY, hv_MeanY, hv_Deviation, hv_Row13;
	HTuple  hv_Column13, hv_Row23, hv_Column23, hv_RowCenter;
	HTuple  hv_MinXa, hv_MaxXa, hv_MinYa, hv_MaxYa, hv_MeanYa;
	HTuple  hv_Row13a, hv_Column13a, hv_Row23a, hv_Column23a;
	HTuple  hv_nPts, hv_ptsXListZeroM, hv_ptsYListZeroM, hv_ptsZListZeroM;
	HTuple  hv_ptsDefListZeroM, hv_ptsRowListZeroM, hv_ptsColListZeroM, hv_ptsIndexLzListZeroM;
	HTuple  hv_Min2, hv_Max2, hv_Range2, hv_DmZ, hv_LzHeight;
	HTuple  hv_MxHeight, hv_fMxZCur, hv_fLastHeight, hv_MxHeightCur,hv_iLayer, hv_bLast, hv_fJgPPZStepCur;
	HTuple  hv_Newtuple, hv_ddd2;

	HTuple hv_j, hv_ptsZMaxRelDmZeroS, hv_ptsMxHeightCurZeroS;
	HTuple hv_ptsZMaxRelDmZeroM, hv_ptsMxHeightCurZeroM;

	HTuple hv_bRight, hv_bRightLastBuff;


	HTuple  hv_Width, hv_Height, hv_RowFirst, hv_ColumnFirst;
	HTuple  hv_RowCur, hv_ColumnCur, hv_Number, hv_I, hv_Area;
	HTuple   hv_NewtupleRow, hv_NewtupleColumn;
	HTuple  hv_Distance, hv_Indices, hv_Area1;
	HTuple  hv_LengthPath4, hv_LengthPath0, hv_LengthPath1;
	HTuple  hv_LengthPath2, hv_LengthPath3, hv_iModeJgPPSortSel;

	(*hv_ptsXList) = HTuple();
	(*hv_ptsYList) = HTuple();
	(*hv_ptsZList) = HTuple();
	(*hv_ptsDefList) = HTuple();

	(*hv_ptsRowList) = HTuple();
	(*hv_ptsColList) = HTuple();

	(*hv_ptsZMaxRelDmList) = HTuple();
	(*hv_ptsMxHeightCurList) = HTuple();

	(*hv_ptsIndexLzList) = HTuple();
	

	GenEmptyObj(&(*ho_RegionMXs));

	GenEmptyObj(&ho_SortedRegions);

	if (0 != (int(hv_iModeJgPPSort == 0)))
	{
		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "true", "row");
	}
	else if (0 != (int(hv_iModeJgPPSort == 1)))
	{
		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "false", "row");
	}
	else if (0 != (int(hv_iModeJgPPSort == 2)))
	{
		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "true", "column");
	}
	else if (0 != (int(hv_iModeJgPPSort == 3)))
	{
		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "false", "column");
	}
	else if (0 != (int(hv_iModeJgPPSort == 4)))
	{

		GetImageSize(ho_ZReal, &hv_Width, &hv_Height);
		hv_RowFirst = 0;
		hv_ColumnFirst = hv_Width;
		hv_RowCur = hv_RowFirst;
		hv_ColumnCur = hv_ColumnFirst;

		CountObj(ho_RectLzs, &hv_Number);
		GenEmptyObj(&ho_RegionRemain);
		ConcatObj(ho_RegionRemain, ho_RectLzs, &ho_RegionRemain);

		{
			HTuple end_val37 = hv_Number;
			HTuple step_val37 = 1;
			for (hv_I = 1; hv_I.Continue(end_val37, step_val37); hv_I += step_val37)
			{

				AreaCenter(ho_RegionRemain, &hv_Area, &hv_Row, &hv_Column);

				TupleGenConst(hv_Row.TupleLength(), hv_RowCur, &hv_NewtupleRow);
				TupleGenConst(hv_Column.TupleLength(), hv_ColumnCur, &hv_NewtupleColumn);
				DistancePp(hv_NewtupleRow, hv_NewtupleColumn, hv_Row, hv_Column, &hv_Distance);
				TupleSortIndex(hv_Distance, &hv_Indices);
				if (0 != (int((hv_Indices.TupleLength()) > 0)))
				{

					SelectObj(ho_RegionRemain, &ho_ObjectSelected, HTuple(hv_Indices[0]) + 1);
					AreaCenter(ho_ObjectSelected, &hv_Area1, &hv_Row1, &hv_Column1);
					hv_RowCur = hv_Row1;
					hv_ColumnCur = hv_Column1;
					GenCrossContourXld(&ho_Cross, hv_RowCur, hv_ColumnCur, 71, 0);

					ConcatObj(ho_SortedRegions, ho_ObjectSelected, &ho_SortedRegions);

					RemoveObj(ho_RegionRemain, &ho_RegionRemain, HTuple(hv_Indices[0]) + 1);

					//dev_clear_window ()
					//dev_display (ZReal)
					//dev_display (RegionRemain)
					//dev_display (Cross)
				}


			}
		}



	}
	else if (0 != (int(hv_iModeJgPPSort == 5)))
	{

		GenEmptyObj(&ho_SortedRegions0);
		GenEmptyObj(&ho_SortedRegions1);
		GenEmptyObj(&ho_SortedRegions2);
		GenEmptyObj(&ho_SortedRegions3);
		GenEmptyObj(&ho_SortedRegions4);

		GetImageSize(ho_ZReal, &hv_Width, &hv_Height);
		hv_RowFirst = 0;
		hv_ColumnFirst = hv_Width;
		hv_RowCur = hv_RowFirst;
		hv_ColumnCur = hv_ColumnFirst;

		CountObj(ho_RectLzs, &hv_Number);
		GenEmptyObj(&ho_RegionRemain);
		ConcatObj(ho_RegionRemain, ho_RectLzs, &ho_RegionRemain);

		{
			HTuple end_val86 = hv_Number;
			HTuple step_val86 = 1;
			for (hv_I = 1; hv_I.Continue(end_val86, step_val86); hv_I += step_val86)
			{

				AreaCenter(ho_RegionRemain, &hv_Area, &hv_Row, &hv_Column);

				TupleGenConst(hv_Row.TupleLength(), hv_RowCur, &hv_NewtupleRow);
				TupleGenConst(hv_Column.TupleLength(), hv_ColumnCur, &hv_NewtupleColumn);
				DistancePp(hv_NewtupleRow, hv_NewtupleColumn, hv_Row, hv_Column, &hv_Distance);
				TupleSortIndex(hv_Distance, &hv_Indices);
				if (0 != (int((hv_Indices.TupleLength()) > 0)))
				{

					SelectObj(ho_RegionRemain, &ho_ObjectSelected, HTuple(hv_Indices[0]) + 1);
					AreaCenter(ho_ObjectSelected, &hv_Area1, &hv_Row1, &hv_Column1);
					hv_RowCur = hv_Row1;
					hv_ColumnCur = hv_Column1;
					GenCrossContourXld(&ho_Cross, hv_RowCur, hv_ColumnCur, 71, 0);
					ConcatObj(ho_SortedRegions, ho_ObjectSelected, &ho_SortedRegions);
					RemoveObj(ho_RegionRemain, &ho_RegionRemain, HTuple(hv_Indices[0]) + 1);

					//dev_clear_window ()
					//dev_display (ZReal)
					//dev_display (RegionRemain)
					//dev_display (Cross)
				}

			}
		}

		ho_SortedRegions4 = ho_SortedRegions;
		AreaCenter(ho_SortedRegions, &hv_Area, &hv_Row, &hv_Column);
		GenContourPolygonXld(&ho_Contour, hv_RowFirst.TupleConcat(hv_Row), hv_ColumnFirst.TupleConcat(hv_Column));
		LengthXld(ho_Contour, &hv_LengthPath4);
		if (HDevWindowStack::IsOpen())
			ClearWindow(HDevWindowStack::GetActive());
		if (HDevWindowStack::IsOpen())
			DispObj(ho_ZReal, HDevWindowStack::GetActive());
		if (HDevWindowStack::IsOpen())
			DispObj(ho_Contour, HDevWindowStack::GetActive());

		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "true", "row");
		ho_SortedRegions0 = ho_SortedRegions;
		AreaCenter(ho_SortedRegions, &hv_Area, &hv_Row, &hv_Column);
		GenContourPolygonXld(&ho_Contour, hv_RowFirst.TupleConcat(hv_Row), hv_ColumnFirst.TupleConcat(hv_Column));
		LengthXld(ho_Contour, &hv_LengthPath0);
		//dev_clear_window ()
		//dev_display (ZReal)
		//dev_display (Contour)

		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "false", "row");
		ho_SortedRegions1 = ho_SortedRegions;
		AreaCenter(ho_SortedRegions, &hv_Area, &hv_Row, &hv_Column);
		GenContourPolygonXld(&ho_Contour, hv_RowFirst.TupleConcat(hv_Row), hv_ColumnFirst.TupleConcat(hv_Column));
		LengthXld(ho_Contour, &hv_LengthPath1);
		//dev_clear_window ()
		//dev_display (ZReal)
		//dev_display (Contour)

		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "true", "column");
		ho_SortedRegions2 = ho_SortedRegions;
		AreaCenter(ho_SortedRegions, &hv_Area, &hv_Row, &hv_Column);
		GenContourPolygonXld(&ho_Contour, hv_RowFirst.TupleConcat(hv_Row), hv_ColumnFirst.TupleConcat(hv_Column));
		LengthXld(ho_Contour, &hv_LengthPath2);
		//dev_clear_window ()
		//dev_display (ZReal)
		//dev_display (Contour)

		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "false", "column");
		ho_SortedRegions3 = ho_SortedRegions;
		AreaCenter(ho_SortedRegions, &hv_Area, &hv_Row, &hv_Column);
		GenContourPolygonXld(&ho_Contour, hv_RowFirst.TupleConcat(hv_Row), hv_ColumnFirst.TupleConcat(hv_Column));
		LengthXld(ho_Contour, &hv_LengthPath3);
		if (HDevWindowStack::IsOpen())
			ClearWindow(HDevWindowStack::GetActive());
		if (HDevWindowStack::IsOpen())
			DispObj(ho_ZReal, HDevWindowStack::GetActive());
		if (HDevWindowStack::IsOpen())
			DispObj(ho_Contour, HDevWindowStack::GetActive());


		// stop(...); only in hdevelop

		if (0 != (HTuple(HTuple(HTuple(int(hv_LengthPath0 <= hv_LengthPath1)).TupleAnd(int(hv_LengthPath0 <= hv_LengthPath2))).TupleAnd(int(hv_LengthPath0 <= hv_LengthPath3))).TupleAnd(int(hv_LengthPath0 <= hv_LengthPath4))))
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions0;
			hv_iModeJgPPSortSel = 0;
		}
		else if (0 != (HTuple(HTuple(HTuple(int(hv_LengthPath1 <= hv_LengthPath0)).TupleAnd(int(hv_LengthPath1 <= hv_LengthPath2))).TupleAnd(int(hv_LengthPath1 <= hv_LengthPath3))).TupleAnd(int(hv_LengthPath1 <= hv_LengthPath4))))
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions1;
			hv_iModeJgPPSortSel = 1;
		}
		else if (0 != (HTuple(HTuple(HTuple(int(hv_LengthPath2 <= hv_LengthPath0)).TupleAnd(int(hv_LengthPath2 <= hv_LengthPath1))).TupleAnd(int(hv_LengthPath2 <= hv_LengthPath3))).TupleAnd(int(hv_LengthPath2 <= hv_LengthPath4))))
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions2;
			hv_iModeJgPPSortSel = 2;
		}
		else if (0 != (HTuple(HTuple(HTuple(int(hv_LengthPath3 <= hv_LengthPath0)).TupleAnd(int(hv_LengthPath3 <= hv_LengthPath1))).TupleAnd(int(hv_LengthPath3 <= hv_LengthPath2))).TupleAnd(int(hv_LengthPath3 <= hv_LengthPath4))))
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions3;
			hv_iModeJgPPSortSel = 3;
		}
		else if (0 != (HTuple(HTuple(HTuple(int(hv_LengthPath4 <= hv_LengthPath0)).TupleAnd(int(hv_LengthPath4 <= hv_LengthPath1))).TupleAnd(int(hv_LengthPath4 <= hv_LengthPath2))).TupleAnd(int(hv_LengthPath4 <= hv_LengthPath3))))
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions4;
			hv_iModeJgPPSortSel = 4;
		}
		else
		{
			GenEmptyObj(&ho_SortedRegions);
			ho_SortedRegions = ho_SortedRegions0;
			hv_iModeJgPPSortSel = 0;
		}


	}
	else
	{
		SortRegion(ho_RectLzs, &ho_SortedRegions, "first_point", "true", "row");
	}



	//计算像素分辨率
	MinMaxGray(ho_RegionTb, ho_Y, 0, &hv_Min, &hv_Max, &hv_Range);
	SmallestRectangle1(ho_RegionTb, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
	hv_fYResolution = hv_Range / ((hv_Row1 - hv_Row2).TupleAbs());
	hv_iJgPPYStepPixelCur = (hv_fJgPPYStep / hv_fYResolution).TupleInt();




	//切割区域的方位


	SmallestRectangle1(ho_RegionTb, &hv_Row1, &hv_Column1, &hv_Row2, &hv_Column2);
	GenRectangle1(&ho_RectangleU, hv_Row1, hv_Column1, (hv_Row1 + hv_Row2) / 2, hv_Column2);
	GenRectangle1(&ho_RectangleD, (hv_Row1 + hv_Row2) / 2, hv_Column1, hv_Row2, hv_Column2);
	GenRectangle1(&ho_RectangleL, hv_Row1, hv_Column1, hv_Row2, (hv_Column1 + hv_Column2) / 2);
	GenRectangle1(&ho_RectangleR, hv_Row1, (hv_Column1 + hv_Column2) / 2, hv_Row2, hv_Column2);
	GenRectangle1(&ho_RectangleC, hv_Row1 + 150, hv_Column1 + 150, hv_Row2 - 150, hv_Column2 - 150);


	CountObj(ho_SortedRegions, &hv_NumLz);
	{
		HTuple end_val45 = hv_NumLz - 1;
		HTuple step_val45 = 1;
		for (hv_iNumLz = 0; hv_iNumLz.Continue(end_val45, step_val45); hv_iNumLz += step_val45)
		{
			//Y方向切块 根据设置的mm进给值计算出切割像素值
			SelectObj(ho_SortedRegions, &ho_LzSelected, hv_iNumLz + 1);


			//分析切割区域的方位 给出下刀方向  ？？？下面判断还存在bug
			hv_iCutDir = 0;

			Intersection(ho_LzSelected, ho_RectangleU, &ho_RegionIntersection);
			AreaCenter(ho_RegionIntersection, &hv_AreaU, &hv_Row, &hv_Column);
			Intersection(ho_LzSelected, ho_RectangleD, &ho_RegionIntersection);
			AreaCenter(ho_RegionIntersection, &hv_AreaD, &hv_Row, &hv_Column);
			Intersection(ho_LzSelected, ho_RectangleR, &ho_RegionIntersection);
			AreaCenter(ho_RegionIntersection, &hv_AreaR, &hv_Row, &hv_Column);
			Intersection(ho_LzSelected, ho_RectangleL, &ho_RegionIntersection);
			AreaCenter(ho_RegionIntersection, &hv_AreaL, &hv_Row, &hv_Column);

			if (0 != (HTuple(HTuple(int(hv_AreaU > hv_AreaL)).TupleAnd(int(hv_AreaU > hv_AreaR))).TupleAnd(int(hv_AreaU > hv_AreaD))))
			{
				hv_iCutDir = 2;
			}
			else if (0 != (HTuple(HTuple(int(hv_AreaD > hv_AreaL)).TupleAnd(int(hv_AreaD > hv_AreaR))).TupleAnd(int(hv_AreaD > hv_AreaU))))
			{
				hv_iCutDir = 3;
			}
			else if (0 != (HTuple(HTuple(int(hv_AreaL >= hv_AreaR)).TupleAnd(int(hv_AreaL >= hv_AreaU))).TupleAnd(int(hv_AreaL >= hv_AreaD))))
			{
				hv_iCutDir = 0;
			}
			else if (0 != (HTuple(HTuple(int(hv_AreaR >= hv_AreaL)).TupleAnd(int(hv_AreaR >= hv_AreaU))).TupleAnd(int(hv_AreaR >= hv_AreaD))))
			{
				hv_iCutDir = 1;

			}
			else
			{
				hv_iCutDir = 1;
			}

			//完全在中间区域 从左侧下刀
			Difference(ho_LzSelected, ho_RectangleC, &ho_RegionDifference);
			AreaCenter(ho_RegionDifference, &hv_AreaC, &hv_Row, &hv_Column);
			if (0 != (int(hv_AreaC == 0)))
			{
				hv_iCutDir = 1;
			}



			PartitionRectangle(ho_LzSelected, &ho_Partitioned, 3000, hv_iJgPPYStepPixelCur);
			CountObj(ho_Partitioned, &hv_NumPart);


			GenEmptyObj(&ho_RegionMxList);
			hv_ptsXListZeroS = HTuple();
			hv_ptsYListZeroS = HTuple();
			hv_ptsZListZeroS = HTuple();
			hv_ptsDefListZeroS = HTuple();

			hv_ptsRowListS = HTuple();
			hv_ptsColListS = HTuple();





			hv_bOdd = 0;
			hv_bRight = 0;
			hv_bRightLastBuff = 0;
			{
				HTuple end_val117 = hv_NumPart - 1;
				HTuple step_val117 = 1;
				for (hv_iPart = 0; hv_iPart.Continue(end_val117, step_val117); hv_iPart += step_val117)
				{



					SelectObj(ho_Partitioned, &ho_ObjectSelected, hv_iPart + 1);



					GetDomain(ho_Z, &ho_Domain);
					Intersection(ho_ObjectSelected, ho_Domain, &ho_RegionIntersection);

					MinMaxGray(ho_RegionIntersection, ho_X, 0, &hv_MinX, &hv_MaxX, &hv_Range);
					MinMaxGray(ho_RegionIntersection, ho_Y, 0, &hv_MinY, &hv_MaxY, &hv_Range);


					Intensity(ho_RegionIntersection, ho_Y, &hv_MeanY, &hv_Deviation);
					SmallestRectangle1(ho_ObjectSelected, &hv_Row13, &hv_Column13, &hv_Row23, &hv_Column23);
					GenRectangle1(&ho_RegionMx, hv_Row13, hv_Column13, hv_Row23, hv_Column23);

					ConcatObj(ho_RegionMxList, ho_RegionMx, &ho_RegionMxList);



					//**规划路径，当前考虑X方向来回模式


					//添加起始空跑点和加工起点
					if (0 != (int(hv_iPart == 0)))
					{

						if (0 != (int(hv_iCutDir == 0)))
						{
							hv_ptsYListZeroS = hv_MeanY;
							hv_ptsZListZeroS = 0;
							hv_ptsXListZeroS = hv_MaxX + hv_fOffsetCutStartPos;

							hv_RowCenter = (hv_Row13 / 2) + (hv_Row23 / 2);
							hv_ptsRowListS = hv_RowCenter;
							hv_ptsColListS = hv_Column23 + (hv_fOffsetCutStartPos / hv_fYResolution);

							GenCrossContourXld(&ho_Cross, hv_ptsRowListS, hv_ptsColListS, 6, 0);

							hv_bRight = 1;
						}

						if (0 != (int(hv_iCutDir == 1)))
						{
							hv_ptsYListZeroS = hv_MeanY;
							hv_ptsZListZeroS = 0;
							hv_ptsXListZeroS = hv_MinX - hv_fOffsetCutStartPos;

							hv_RowCenter = (hv_Row13 / 2) + (hv_Row23 / 2);
							hv_ptsRowListS = hv_RowCenter;
							hv_ptsColListS = hv_Column13 - (hv_fOffsetCutStartPos / hv_fYResolution);


							GenCrossContourXld(&ho_Cross, hv_ptsRowListS, hv_ptsColListS, 6, 0);

							hv_bRight = 0;
						}


						if (0 != (int(hv_iCutDir == 2)))
						{

							//下侧下刀要考虑一片区域的干涉
							GetDomain(ho_Z, &ho_Domain);
							Intersection(ho_LzSelected, ho_Domain, &ho_RegionIntersection);

							MinMaxGray(ho_RegionIntersection, ho_X, 0, &hv_MinXa, &hv_MaxXa, &hv_Range);
							MinMaxGray(ho_RegionIntersection, ho_Y, 0, &hv_MinYa, &hv_MaxYa, &hv_Range);

							Intensity(ho_RegionIntersection, ho_Y, &hv_MeanYa, &hv_Deviation);
							SmallestRectangle1(ho_LzSelected, &hv_Row13a, &hv_Column13a, &hv_Row23a,
								&hv_Column23a);

							hv_ptsYListZeroS = hv_MaxYa + hv_fOffsetCutStartPos;
							hv_ptsZListZeroS = 0;
							hv_ptsXListZeroS = hv_MinX - hv_fOffsetBothEndsL;

							hv_ptsRowListS = hv_Row23a + (hv_fOffsetCutStartPos / hv_fYResolution);
							hv_ptsColListS = hv_Column13a - (hv_fOffsetBothEndsL / hv_fYResolution);

							GenCrossContourXld(&ho_Cross, hv_ptsRowListS, hv_ptsColListS, 6, 0);

							hv_bRight = 0;

						}


						if (0 != (int(hv_iCutDir == 3)))
						{
							hv_ptsYListZeroS = hv_MinY - hv_fOffsetCutStartPos;
							hv_ptsZListZeroS = 0;
							hv_ptsXListZeroS = hv_MinX - hv_fOffsetBothEndsL;

							hv_ptsRowListS = hv_Row13 - (hv_fOffsetCutStartPos / hv_fYResolution);
							hv_ptsColListS = hv_Column13 - (hv_fOffsetBothEndsL / hv_fYResolution);

							GenCrossContourXld(&ho_Cross, hv_ptsRowListS, hv_ptsColListS, 50, 0);

							hv_bRight = 0;
						}

						hv_ptsDefListZeroS = 4;

						hv_ptsYListZeroS = hv_ptsYListZeroS.TupleConcat(hv_ptsYListZeroS);
						hv_ptsZListZeroS = hv_ptsZListZeroS.TupleConcat(hv_ptsZListZeroS);
						hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(hv_ptsXListZeroS);
						hv_ptsRowListS = hv_ptsRowListS.TupleConcat(hv_ptsRowListS);
						hv_ptsColListS = hv_ptsColListS.TupleConcat(hv_ptsColListS);
						hv_ptsDefListZeroS = hv_ptsDefListZeroS.TupleConcat(1);

					}


					//读取上一次加工点在左边还是右边
					hv_bRightLastBuff = hv_bRight;


					//添加加工点 --含路径加工终点
					hv_ptsYListZeroS = hv_ptsYListZeroS.TupleConcat(hv_MeanY);
					hv_ptsZListZeroS = hv_ptsZListZeroS.TupleConcat(0);
					hv_ptsYListZeroS = hv_ptsYListZeroS.TupleConcat(hv_MeanY);
					hv_ptsZListZeroS = hv_ptsZListZeroS.TupleConcat(0);

					hv_RowCenter = (hv_Row13 / 2) + (hv_Row23 / 2);
					hv_ptsRowListS = hv_ptsRowListS.TupleConcat(hv_RowCenter);
					hv_ptsRowListS = hv_ptsRowListS.TupleConcat(hv_RowCenter);

					//如果是右侧下刀 起始点 20251025


					if (0 != (int(hv_bRightLastBuff == 0)))
					{
						hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(hv_MinX - hv_fOffsetBothEndsL);
						hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(hv_MaxX + hv_fOffsetBothEndsR);

						hv_ptsColListS = hv_ptsColListS.TupleConcat(hv_Column13 - (hv_fOffsetBothEndsL / hv_fYResolution));
						hv_ptsColListS = hv_ptsColListS.TupleConcat(hv_Column23 + (hv_fOffsetBothEndsR / hv_fYResolution));

						hv_bRight = 1;
					}
					else
					{
						hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(hv_MaxX + hv_fOffsetBothEndsR);
						hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(hv_MinX - hv_fOffsetBothEndsL);

						hv_ptsColListS = hv_ptsColListS.TupleConcat(hv_Column23 + (hv_fOffsetBothEndsR / hv_fYResolution));
						hv_ptsColListS = hv_ptsColListS.TupleConcat(hv_Column13 - (hv_fOffsetBothEndsL / hv_fYResolution));

						hv_bRight = 0;

					}

					GenCrossContourXld(&ho_Cross, hv_ptsRowListS, hv_ptsColListS, 6, 0);


					hv_ptsDefListZeroS = hv_ptsDefListZeroS.TupleConcat(3);
					hv_ptsDefListZeroS = hv_ptsDefListZeroS.TupleConcat(3);


				}
			}

			//最后一个点写为终点
			hv_ptsDefListZeroS[(hv_ptsXListZeroS.TupleLength()) - 1] = 2;

			//终点之后需要增加一个空跑点
			hv_nPts = hv_ptsXListZeroS.TupleLength();
			hv_ptsXListZeroS = hv_ptsXListZeroS.TupleConcat(HTuple(hv_ptsXListZeroS[hv_nPts - 1]));
			hv_ptsYListZeroS = hv_ptsYListZeroS.TupleConcat(HTuple(hv_ptsYListZeroS[hv_nPts - 1]));
			hv_ptsZListZeroS = hv_ptsZListZeroS.TupleConcat(HTuple(hv_ptsZListZeroS[hv_nPts - 1]));
			hv_ptsDefListZeroS = hv_ptsDefListZeroS.TupleConcat(4);
			hv_ptsRowListS = hv_ptsRowListS.TupleConcat(HTuple(hv_ptsRowListS[hv_nPts - 1]));
			hv_ptsColListS = hv_ptsColListS.TupleConcat(HTuple(hv_ptsColListS[hv_nPts - 1]));




			//包含Z深度方向多层路径规划
			hv_ptsXListZeroM = HTuple();
			hv_ptsYListZeroM = HTuple();
			hv_ptsZListZeroM = HTuple();
			hv_ptsDefListZeroM = HTuple();

			//增加当前粒子索引
			hv_ptsIndexLzListZeroM = HTuple();

			hv_ptsRowListZeroM = HTuple();
			hv_ptsColListZeroM = HTuple();

			hv_ptsZMaxRelDmZeroM = HTuple();
			hv_ptsMxHeightCurZeroM = HTuple();

			//粒子绝对高度
			MinMaxGray(ho_LzSelected, ho_ZReal, 0, &hv_Min, &hv_Max, &hv_Range);
			//粒子相对高度
			MinMaxGray(ho_LzSelected, ho_ZSub, 0, &hv_Min2, &hv_Max2, &hv_Range2);

			//粒子底部相对于零平面的高度
			hv_DmZ = hv_Max - hv_Max2;//后续需要考虑底面高度计算不准的情况

			hv_LzHeight = hv_Max;
			hv_MxHeight = ((hv_LzHeight + hv_fJgLzUpOffset) - hv_fJgDpUpOffset) - hv_DmZ;
			hv_fMxZCur = hv_LzHeight /*+ hv_fJgLzUpOffset*/;//20251118 考虑一下粒子上抬

			//空跑点和起始点处理


			//中间点高度处理
			for (hv_iLayer = 0; hv_iLayer <= 100; hv_iLayer += 1)
			{

				hv_bLast = 0;

				//计算当前进给Z
				hv_fJgPPZStepCur = 1.0;
				if (0 != (int(hv_iModeJgPPZStepMode == 0)))
				{
					hv_fJgPPZStepCur = hv_fJgPPZStepFix;
				}
				else
				{
					hv_fJgPPZStepCur = (hv_fJgPPZStepK * hv_fMxZCur) + hv_fJgPPZStepB;

					if (0 != (int(hv_fJgPPZStepCur < hv_fJgPPZStepMin)))
					{
						hv_fJgPPZStepCur = hv_fJgPPZStepMin;
					}


					if (0 != (int(hv_fJgPPZStepCur > hv_fJgPPZStepMax)))
					{
						hv_fJgPPZStepCur = hv_fJgPPZStepMax;
					}


				}

				hv_fLastHeight = hv_fMxZCur;

				hv_fMxZCur = hv_fMxZCur - hv_fJgPPZStepCur;
				if (0 != (int(hv_fMxZCur < (hv_fJgDpUpOffset + hv_DmZ))))
				{
					hv_fMxZCur = hv_fJgDpUpOffset + hv_DmZ;
					hv_bLast = 1;
				}

				//*磨完这一次，下次只剩下很小的高度 直接磨完
				if (0 != (int((hv_fMxZCur - (hv_fJgDpUpOffset + hv_DmZ)) < 1.0)))
				{
					hv_fMxZCur = hv_fJgDpUpOffset + hv_DmZ;
					hv_bLast = 1;
				}


				hv_MxHeightCur = hv_fLastHeight - hv_fMxZCur;

				//添加到区域路径点
				hv_ptsXListZeroM = hv_ptsXListZeroM.TupleConcat(hv_ptsXListZeroS);
				hv_ptsYListZeroM = hv_ptsYListZeroM.TupleConcat(hv_ptsYListZeroS);
				TupleGenConst(hv_ptsZListZeroS.TupleLength(), hv_fMxZCur * -1.0, &hv_Newtuple);
				

				//空跑点高度做一次修改20251119 修改成当前最大高度+上抬高度
				//hv_Newtuple[0] = (-hv_LzHeight) - hv_fJgTdZUp;
				//hv_Newtuple[(hv_ptsXListZeroS.TupleLength()) - 1] = (-hv_LzHeight) - hv_fJgTdZUp;
				hv_Newtuple[0] = (-hv_fLastHeight) - hv_fJgTdZUp;
				hv_Newtuple[(hv_ptsXListZeroS.TupleLength()) - 1] = (-hv_fLastHeight) - hv_fJgTdZUp;
				
				
				hv_ptsZListZeroM = hv_ptsZListZeroM.TupleConcat(hv_Newtuple);
				hv_ptsDefListZeroM = hv_ptsDefListZeroM.TupleConcat(hv_ptsDefListZeroS);

				hv_ptsRowListZeroM = hv_ptsRowListZeroM.TupleConcat(hv_ptsRowListS);
				hv_ptsColListZeroM = hv_ptsColListZeroM.TupleConcat(hv_ptsColListS);


				hv_ptsZMaxRelDmZeroS = HTuple();
				hv_ptsMxHeightCurZeroS = HTuple();
				TupleGenConst(hv_ptsDefListZeroS.TupleLength(), 0, &hv_ptsZMaxRelDmZeroS);
				TupleGenConst(hv_ptsDefListZeroS.TupleLength(), 0, &hv_ptsMxHeightCurZeroS);

				//添加加工最高点和当前加工深度的信息
				{
					HTuple end_val361 = (hv_ptsDefListZeroS.TupleLength()) - 1;
					HTuple step_val361 = 1;
					for (hv_j = 0; hv_j.Continue(end_val361, step_val361); hv_j += step_val361)
					{
						if (0 != (int(HTuple(hv_ptsDefListZeroS[hv_j]) != 4)))
						{
							hv_ptsZMaxRelDmZeroS[hv_j] = hv_LzHeight;
							hv_ptsMxHeightCurZeroS[hv_j] = hv_MxHeightCur;
						}
					}
				}
				hv_ptsZMaxRelDmZeroM = hv_ptsZMaxRelDmZeroM.TupleConcat(hv_ptsZMaxRelDmZeroS);
				hv_ptsMxHeightCurZeroM = hv_ptsMxHeightCurZeroM.TupleConcat(hv_ptsMxHeightCurZeroS);

				//增加当前粒子索引
				TupleGenConst(hv_ptsDefListZeroS.TupleLength(), hv_iNumLz, &hv_Newtuple);
				hv_ptsIndexLzListZeroM = hv_ptsIndexLzListZeroM.TupleConcat(hv_Newtuple);


				if (0 != hv_bLast)
				{

					break;
				}

			}




			//输出--路径结果
			(*hv_ptsXList) = (*hv_ptsXList).TupleConcat(hv_ptsXListZeroM);
			(*hv_ptsYList) = (*hv_ptsYList).TupleConcat(hv_ptsYListZeroM);
			(*hv_ptsZList) = (*hv_ptsZList).TupleConcat(hv_ptsZListZeroM);
			(*hv_ptsDefList) = (*hv_ptsDefList).TupleConcat(hv_ptsDefListZeroM);


			(*hv_ptsRowList) = (*hv_ptsRowList).TupleConcat(hv_ptsRowListZeroM);
			(*hv_ptsColList) = (*hv_ptsColList).TupleConcat(hv_ptsColListZeroM);

			(*hv_ptsZMaxRelDmList) = (*hv_ptsZMaxRelDmList).TupleConcat(hv_ptsZMaxRelDmZeroM);
			(*hv_ptsMxHeightCurList) = (*hv_ptsMxHeightCurList).TupleConcat(hv_ptsMxHeightCurZeroM);

			//增加当前粒子索引
			(*hv_ptsIndexLzList) = (*hv_ptsIndexLzList).TupleConcat(hv_ptsIndexLzListZeroM);

			//输出--区域
			ConcatObj((*ho_RegionMXs), ho_RegionMxList, &(*ho_RegionMXs));

			hv_ddd = 0;

		}
	}


	hv_ddd2 = 0;

	return;
}



void  CPlateB:: trans_pose_face_x(HTuple hv_Pose1, HTuple hv_bFaceOrg, HTuple* hv_Pose2)
{

    // Local iconic variables

    // Local control variables
    HTuple  hv_PlaneDatum, hv_SampledObjectModel3D;
    HTuple  hv_ObjectModel3DNormals, hv_valNx, hv_valNy, hv_valNz;
    HTuple  hv_ValCosX, hv_ValCosY, hv_ValCosZ, hv_OriginX;
    HTuple  hv_OriginY, hv_OriginZ, hv_TargetX, hv_TargetY;
    HTuple  hv_TargetZ, hv_HomMat3D;





    GenPlaneObjectModel3d(hv_Pose1, (((HTuple(-1).Append(-1)).Append(1)).Append(1)) * 50,
        (((HTuple(-1).Append(1)).Append(1)).Append(-1)) * 50, &hv_PlaneDatum);

    SampleObjectModel3d(hv_PlaneDatum, "fast", 5, HTuple(), HTuple(), &hv_SampledObjectModel3D);
    SurfaceNormalsObjectModel3d(hv_SampledObjectModel3D, "mls", "mls_force_inwards",
        "true", &hv_ObjectModel3DNormals);
    //visualize_object_model_3d (WindowHandle, [ObjectModel3DNormals], [], [], [ 'lut','color_attrib','normal_color','point_size','disp_normals','disp_pose'], ['color1','coord_z','white',16,'true','true'], [], [], [], Pose2)


    GetObjectModel3dParams(hv_ObjectModel3DNormals, "point_normal_x", &hv_valNx);
    GetObjectModel3dParams(hv_ObjectModel3DNormals, "point_normal_y", &hv_valNy);
    GetObjectModel3dParams(hv_ObjectModel3DNormals, "point_normal_z", &hv_valNz);


    //平面拟合后的法向是朝向原点的 】

    if (0 != hv_bFaceOrg)
    {

        TupleMean(hv_valNx, &hv_ValCosX);
        TupleMean(hv_valNy, &hv_ValCosY);
        TupleMean(hv_valNz, &hv_ValCosZ);

    }
    else
    {


        TupleMean(-hv_valNx, &hv_ValCosX);
        TupleMean(-hv_valNy, &hv_ValCosY);
        TupleMean(-hv_valNz, &hv_ValCosZ);
    }





    hv_OriginX.Clear();
    hv_OriginX[0] = 0;
    hv_OriginX[1] = 0;
    hv_OriginX[2] = 0;
    hv_OriginY.Clear();
    hv_OriginY[0] = 0;
    hv_OriginY[1] = 0;
    hv_OriginY[2] = 0;
    hv_OriginZ.Clear();
    hv_OriginZ[0] = 0;
    hv_OriginZ[1] = 0.5;
    hv_OriginZ[2] = 1;
    hv_TargetX.Clear();
    hv_TargetX[0] = 0;
    hv_TargetX.Append(hv_ValCosX / 2);
    hv_TargetX.Append(hv_ValCosX);
    hv_TargetY.Clear();
    hv_TargetY[0] = 0;
    hv_TargetY.Append(hv_ValCosY / 2);
    hv_TargetY.Append(hv_ValCosY);
    hv_TargetZ.Clear();
    hv_TargetZ[0] = 0;
    hv_TargetZ.Append(hv_ValCosZ / 2);
    hv_TargetZ.Append(hv_ValCosZ);


    VectorToHomMat3d("rigid", hv_OriginX, hv_OriginY, hv_OriginZ, hv_TargetX, hv_TargetY,
        hv_TargetZ, &hv_HomMat3D);
    HomMat3dToPose(hv_HomMat3D, &(*hv_Pose2));

    (*hv_Pose2)[0] = HTuple(hv_Pose1[0]);
    (*hv_Pose2)[1] = HTuple(hv_Pose1[1]);
    (*hv_Pose2)[2] = HTuple(hv_Pose1[2]);

    (*hv_Pose2)[5] = 0;
    //gen_arrow_object_model_3d_x (2, 500, Pose1, OM3DArrow2)
    //visualize_object_model_3d (WindowHandle, [PlaneDatum,OM3DArrow2], [], [], ['lut','color_attrib','disp_pose'], ['color1','coord_z','true'], [], [], [], Pose2)

    ClearObjectModel3d(hv_PlaneDatum);
    ClearObjectModel3d(hv_SampledObjectModel3D);
    ClearObjectModel3d(hv_ObjectModel3DNormals);


    return;
}


void CPlateB::connection_3d_region_by_dis_x(HObject ho_X, HObject ho_Y, HObject ho_Z, HObject ho_Region,
	HObject* ho_RegionPart, HObject* ho_RectPart, HTuple hv_Dis)
{

	// Local iconic variables
	HObject  ho_RegionUnion, ho_ImageReduced, ho_XLz;
	HObject  ho_YLz, ho_ZLz, ho_Domain;

	// Local control variables
	HTuple  hv_OM3dLz, hv_ObjectModel3DConnected;
	HTuple  hv_i, hv_Number, hv_Row11, hv_Column11, hv_Row2;
	HTuple  hv_Column2;


	GenEmptyObj(&(*ho_RegionPart));
	GenEmptyObj(&(*ho_RectPart));

	Union1(ho_Region, &ho_RegionUnion);
	ReduceDomain(ho_Z, ho_RegionUnion, &ho_ImageReduced);

	XyzToObjectModel3d(ho_X, ho_Y, ho_ImageReduced, &hv_OM3dLz);
	ConnectionObjectModel3d(hv_OM3dLz, "distance_3d", hv_Dis, &hv_ObjectModel3DConnected);


	{
		HTuple end_val11 = (hv_ObjectModel3DConnected.TupleLength()) - 1;
		HTuple step_val11 = 1;
		for (hv_i = 0; hv_i.Continue(end_val11, step_val11); hv_i += step_val11)
		{
			ObjectModel3dToXyz(&ho_XLz, &ho_YLz, &ho_ZLz, HTuple(hv_ObjectModel3DConnected[hv_i]),
				"from_xyz_map", HTuple(), HTuple());

			GetDomain(ho_XLz, &ho_Domain);
			ConcatObj((*ho_RegionPart), ho_Domain, &(*ho_RegionPart));

		}
	}

	CountObj((*ho_RegionPart), &hv_Number);
	if (0 != (int(hv_Number > 0)))
	{
		SmallestRectangle1((*ho_RegionPart), &hv_Row11, &hv_Column11, &hv_Row2, &hv_Column2);
		GenRectangle1(&(*ho_RectPart), hv_Row11, hv_Column11, hv_Row2, hv_Column2);

	}


	//联通再处理
	Union1((*ho_RectPart), &ho_RegionUnion);
	Connection(ho_RegionUnion, &(*ho_RectPart));



	return;
}

